/*! \file
 * \brief The math library of the myth package.
 *
 * Interface of the Myth Library.
 * Another thing. Lol.
 * Third line.
 */

#ifndef __MYTH_MATH_HPP_INCLUDED__
#define __MYTH_MATH_HPP_INCLUDED__

#include "Myth.hpp"

#include <math.h>
#include <float.h>
#include <stdlib.h> // for abs() etc.
#include <limits.h> // For INT_MAX / UINT_MAX

#ifndef FLT_MAX
#define FLT_MAX 3.402823466E+38F
#endif

#ifdef PI // make sure we don't collide with a define
#undef PI
#endif
#ifdef PI64
#undef PI64
#endif

namespace Myth
{
//! The math namespace
namespace Math
{

//! The rounding error used in calculations with 32 bit floating point values.
const float ROUNDING_ERROR_32 = 0.00001f;

//! The rounding error used in calculations with 64 bit floating point values.
const double ROUNDING_ERROR_64 = 0.00000001;

//! Constant for PI.
const float PI = 3.14159265359f;

//! Constant for 64bit PI.
const double PI64 = 3.1415926535897932384626433832795028841971693993751;

//! Constant for reciprocal of PI.
const float RECIPROCAL_PI = (1.0f / PI);

//! Constant for 64bit reciprocal of PI.
const double RECIPROCAL_PI64 = (1.0 / PI64);

//! Constant for half of PI.
const float HALF_PI = (PI * 0.5f);

//! Constant for 64 bit half of PI.
const double HALF_PI64 = (PI64 * 0.5);

//! 32bit Constant for converting from degrees to radians.
const float DEGTORAD = (PI / 180.0f);

//! 32bit constant for converting from radians to degrees (formally known as GRAD_PI).
const float RADTODEG = (180.0f / PI);

//! 64bit constant for converting from degrees to radians (formally known as GRAD_PI2).
const double DEGTORAD64 = (PI64 / 180.0);

//! 64bit constant for converting from radians to degrees.
const double RADTODEG64 = (180.0 / PI64);

//! Utility function to convert a radians value to degrees.
/*!
 * \return The angle in degrees.
 * \param[in] radians The angle in radians.
 */
inline float rad_to_deg(float radians)
{
    return RADTODEG * radians;
}

//! Utility function to convert a radians value to degrees.
/// \return double
///         The angle in degrees.
/// \param   double radians
///         The angle in radians.
inline double rad_to_deg( double radians)
{
    return RADTODEG64 * radians;
}

//! Utility function to convert a degrees value to radians.
/// \return float
///         The angle in radians.
/// \param   float degrees
///         The angle in degrees.
inline float deg_to_rad( float degrees)
{
    return DEGTORAD * degrees;
}

//! Utility function to convert a degrees value to radians.
/// \return double
///         The angle in radians.
/// \param   double degrees
///         The angle in degrees.
inline double deg_to_rad( double degrees)
{
    return DEGTORAD64 * degrees;
}

//! Return the smaller of two values.
/// \return T
///         The smaller of the two values.
/// \param   const T& a
///         The first value to check with.
/// \param   const T& b
///         The second value to check with.
template<class T>
inline T min(const T& a, const T& b)
{
    return a < b ? a : b;
}

//! Return the smaller of two values.
/// \return int32_t
///         The smaller of the two values.
/// \param   const int32_t& a
///         The first value to check with.
/// \param   const int32_t& b
///         The second value to check with.
template<>
inline int32_t min( const int32_t& a,
                    const int32_t& b)
{
    const int32_t mask = (a - b) >> 31;
    return (a & mask) | (b & ~mask);
}

//! Return the smaller of three values.
/// \return T
///         The smaller of the three values.
/// \param   const T& a
///         The first value to check with.
/// \param   const T& b
///         The second value to check with.
/// \param   const T& c
///         The third value to check with.
template<class T>
inline T min( const T& a,
              const T& b,
              const T& c)
{
    return a < b ? min(a, c) : min(b, c);
}

/** inline function **/
/** max             **/
//! Return the larger of two values.
/// \return T
///         The larger of the two values.
/// \param   const T& a
///         The first value to check with.
/// \param   const T& b
///         The second value to check with.
template<class T>
inline T max( const T& a,
              const T& b)
{
    return a < b ? b : a;
}

/** inline function **/
/** max<int32_t>        **/
//! Return the larger of two values.
/// \return int32_t
///         The larger of the two values.
/// \param   const int32_t& a
///         The first value to check with.
/// \param   const int32_t& b
///         The second value to check with.
inline int32_t max( const int32_t& a,
                    const int32_t& b)
{
    const int32_t mask = (a - b) >> 31;
    return (b & mask) | (a & ~mask);
}

/** inline function **/
/** max             **/
//! Return the larger of three values.
/// \return T
///         The larger of the three values.
/// \param   const T& a
///         The first value to check with.
/// \param   const T& b
///         The second value to check with.
/// \param   const T& c
///         The third value to check with.
template<class T>
inline T max( const T& a,
              const T& b,
              const T& c)
{
    return a < b ? max(b, c) : max(a, c);
}

/** inline function **/
/** abs             **/
//! Returns the absolute value of a number.
/// \return T
///         The absolute value.
/// \param   const T& a
///         The value you want the absolute from.
template<class T>
inline T abs( const T& a)
{
    return a < (T)0 ? -a : a;
}

/** inline function **/
/** lerp            **/
//! Returns linear interpolation of a and b with ratio t.
/// \return T
///         a if t==0, b if t==1, and a linear interpolated value otherwise.
/// \param   const T& a
///         First value to interpolate.
/// \param   const T& b
///         Second value to interpolate.
/// \param   const Q& t
///         Ratio of interpolation.
template<class T, class Q>
inline T lerp( const T& a,
               const T& b,
               const Q& t)
{
    return (T)(a*((Q)1-t)) + (b*t);
}

/** inline function **/
/** clamp           **/
//! Clamps a value between low and high.
/// \return T
///         The clamped value.
/// \param   const T& value
///         The value you want to clamp.
/// \param   const T& low
///         The lower-barrier.
/// \param   const T& high
///         The higher-barrier.
template <class T>
inline T clamp( const T& value,
                const T& low,
                const T& high)
{
    return min(max(value,low), high);
}

/** inline function **/
/** swap            **/
//! Swaps the content of the passed parameters.
/// \return void
/// \param   T& a
///         The first pointer.
/// \param   T& b
///         The second pointer.
template <class T>
inline void swap( T& a,
                  T& b)
{
    T c(a);
    a = b;
    b = c;
}

/** inline function **/
/** average         **/
//! Calculates the average of an array of objects.
/// \return T
///         The average.
/// \param   T* array
///         Pointer to an array of objects.
/// \param   const Q& count
///         The size of the array.
template<class T, class Q>
inline T average( T* array,
                  const Q& count)
{
    if(!count)
        return T(0);

    T sum = T(0);
    for(Q i = 0; i < count; i++)
        sum += array[i];

    return T(sum/T(count));
}

/** inline function **/
/** equals          **/
//! Returns if a equals b, taking possible rounding errors into account.
/// \return bool
///         True if equal, false otherwise (respects rounding error).
/// \param   const T& a
///         First value to check with.
/// \param   const T& b
///         Second value to check with.
/// \param   const T& tolerance
///         Tolerated rounding error.
template<typename T>
inline bool equals( const T& a,
                    const T& b,
                    const T& tolerance)
{
    return (a + tolerance >= b) && (a - tolerance <= b);
}

/** inline function **/
/** equals          **/
//! Returns if a equals b.
/// \return bool
///         True if equal, false otherwise.
/// \param   const T& a
///         First value to check with.
/// \param   const T& b
///         Second value to check with.
template<typename T>
inline bool equals( const T& a,
                    const T& b)
{
    return a == b;
}

/** inline function **/
/** equals<float>     **/
//! Returns if a equals b, taking possible rounding errors into account.
/// \return bool
///         True if equal, false otherwise (respects rounding error).
/// \param   const float& a
///         First value to check with.
/// \param   const float& b
///         Second value to check with.
template<>
inline bool equals<float>( const float& a,
                           const float& b)
{
    return (a + ROUNDING_ERROR_32 >= b) && (a - ROUNDING_ERROR_32 <= b);
}

/** inline function **/
/** equals<double>     **/
//! Returns if a equals b, taking possible rounding errors into account.
/// \return bool
///         True if equal, false otherwise (respects rounding error).
/// \param   const double& a
///         First value to check with.
/// \param   const double& b
///         Second value to check with.
template<>
inline bool equals<double>( const double& a,
                            const double& b)
{
    return (a + ROUNDING_ERROR_64 >= b) && (a - ROUNDING_ERROR_64 <= b);
}

/** inline function **/
/** is_zero         **/
//! Returns if a equals zero, taking rounding errors into account.
/// \return bool
///         True if zero, false otherwise (respects rounding error).
/// \param   const T& a
///         Value to check if zero.
/// \param   const T& tolerance
///         Tolerated rounding error.
template<typename T>
inline bool is_zero( const T& a,
                     const T& tolerance)
{
    return equals(a, (T)0, tolerance);
}

/** inline function **/
/** is_zero<float>    **/
//! Returns if a equals zero, taking rounding errors into account.
/// \return bool
///         True if zero, false otherwise (respects rounding error).
/// \param   const float& a
///         Value to check if zero.
/// \param   const float& tolerance
///         Tolerated rounding error.
template<>
inline bool is_zero<float>( const float& a,
                            const float& tolerance)
{
    return fabsf(a) <= tolerance;
}

/** inline function **/
/** is_zero<double>    **/
//! Returns if a equals zero, taking rounding errors into account.
/// \return bool
///         True if zero, false otherwise (respects rounding error).
/// \param   const double a
///         Value to check if zero.
/// \param   const double tolerance
///         Tolerated rounding error.
template<>
inline bool is_zero<double>( const double& a,
                             const double& tolerance)
{
    return fabs(a) <= tolerance;
}

/** inline function **/
/** is_zero<int32_t>    **/
//! Returns if a equals zero, taking rounding errors into account.
/// \return bool
///         True if zero, false otherwise (respects rounding error).
/// \param   const int32_t& a
///         Value to check if zero.
/// \param   const int32_t& tolerance
///         Tolerated rounding error.
template<>
inline bool is_zero<int32_t>( const int32_t& a,
                              const int32_t& tolerance)
{
    return (a & 0x7ffffff) <= tolerance;
}

/** inline function **/
/** is_zero<uint32_t>    **/
//! Returns if a equals zero, taking rounding errors into account.
/// \return bool
///         True if zero, false otherwise (respects rounding error).
/// \param   const uint32_t& a
///         Value to check if zero.
/// \param   const uint32_t& tolerance
///         Tolerated rounding error.
template<>
inline bool is_zero<uint32_t>( const uint32_t& a,
                               const uint32_t& tolerance)
{
    return a <= tolerance;
}

/** inline function **/
/** is_zero         **/
//! Returns if a equals zero.
/// \return bool
///         True if zero, false otherwise.
/// \param   const T& a
///         Value to check if zero.
template<typename T>
inline bool is_zero( const T& a)
{
    return a == (T)0;
}

/** inline function **/
/** is_zero<float>    **/
//! Returns if a equals zero.
/// \return bool
///         True if zero, false otherwise.
/// \param   const float& a
///         Value to check if zero.
template<>
inline bool is_zero<float>( const float& a)
{
    return fabsf(a) <= ROUNDING_ERROR_32;
}

/** inline function **/
/** is_zero<double>    **/
//! Returns if a equals zero.
/// \return bool
///         True if zero, false otherwise.
/// \param   const double& a
///         Value to check if zero.
template<>
inline bool is_zero<double>( const double& a)
{
    return fabs(a) <= ROUNDING_ERROR_64;
}

/** inline function **/
/** if_c_a_else_b   **/
//! Conditional set based on mask and arithmetic shift.
/// \return uint32_t
///         if(c) a; else b;
/// \param   int32_t condition
///         The if() condition.
/// \param   uint32_t a
///         The first value.
/// \param   uint32_t b
///         The second value.
inline uint32_t if_c_a_else_b( int32_t condition,
                               uint32_t a,
                               uint32_t b)
{
    return ((-condition >> 31) & (a ^ b)) ^ b;
}

/** inline function **/
/** if_c_a_else_b   **/
//! Conditional set based on mask and arithmetic shift.
/// \return uint16_t
///         if(c) a; else b;
/// \param   int16_t condition
///         The if() condition.
/// \param   uint16_t a
///         The first value.
/// \param   uint16_t b
///         The second value.
inline uint16_t if_c_a_else_b( int16_t condition,
                               uint16_t a,
                               uint16_t b)
{
    return ((-condition >> 15) & (a ^ b)) ^ b;
}

/** inline function **/
/** if_c_a_else_0   **/
//! Conditional set based on mask and arithmetic shift.
/// \return uint32_t
///         a if c is true, 0 otherwise.
/// \param   int32_t condition
///         The if() condition.
/// \param   uint32_t a
///         The 'a' value.
inline uint32_t if_c_a_else_0( int32_t condition,
                               uint32_t a)
{
    return (-condition >> 31) & a;
}

/** inline function **/
/** setbit_cond     **/
//! if (condition) state |= m; else state &= ~m;
/// \return void
/// \param   uint32_t& state
///         The state you might want to change.
/// \param   int32_t condition
///         The if() condition.
/// \param   uint32_t mask
///         The mask to apply.
inline void setbit_cond( uint32_t& state,
                         int32_t condition,
                         uint32_t mask)
{
    state ^= ((-condition >> 31) ^ state) & mask;     // 0, or any postive to mask
}

/** inline function **/
/** round           **/
//! Calculate the rounded value of x.
/// \return T
///         The rounded value of x.
/// \param   const T& x
///         The value you want to round.
template<typename T>
inline T round(const T& x)
{
    return (T)floorf(x + 0.5f);
}

/** inline function **/
/** squareroot      **/
//! Calculate the squareroot of x.
/// \return float
///         The squareroot of x.
/// \param   float x
///         The value you want the squareroot of.
inline float squareroot( float x)
{
    return sqrtf(x);
}

/** inline function **/
/** squareroot      **/
//! Calculate the squareroot of x.
/// \return double
///         The squareroot of x.
/// \param   double x
///         The value you want the squareroot of.
inline double squareroot( double x)
{
    return sqrt(x);
}

/** inline function **/
/** squareroot      **/
//! Calculate the squareroot of x.
/// \return int32_t
///         The squareroot of x.
/// \param   int32_t x
///         The value you want the squareroot of.
inline int32_t squareroot( int32_t x)
{
    return static_cast<int32_t>(squareroot(static_cast<float>(x)));
}

/// \param x: The value you want the reciprocal squareroot of.

/** inline function       **/
/** reciprocal_squareroot **/
//! Calculate the reciprocal squareroot of x (1/sqrt(x)).
/// \return double
///         The reciprocal squareroot of x.
/// \param   double x
///         The value you want the reciprocal squareroot of.
inline double reciprocal_squareroot( double x)
{
    return 1.0 / sqrt(x);
}

/** inline function       **/
/** reciprocal_squareroot **/
//! Calculate the reciprocal squareroot of x (1/sqrt(x)).
/// \return float
///         The reciprocal squareroot of x.
/// \param   float x
///         The value you want the reciprocal squareroot of.
inline float reciprocal_squareroot( float x)
{
    return 1.f / sqrtf(x);
}

/** inline function       **/
/** reciprocal_squareroot **/
//! Calculate the reciprocal squareroot of x (1/sqrt(x)).
/// \return int32_t
///         The reciprocal squareroot of x.
/// \param   int32_t x
///         The value you want the reciprocal squareroot of.
inline int32_t reciprocal_squareroot( int32_t x)
{
    return static_cast<int32_t>(reciprocal_squareroot(static_cast<float>(x)));
}

/** inline function **/
/** reciprocal      **/
//! Calculate the reciprocal of x (1/x).
/// \return float
///         The reciprocal of x.
/// \param   float x
///         The value you want the reciprocal of.
inline float reciprocal( float x)
{
    return 1.f / x;
}

/** inline function **/
/** reciprocal      **/
//! Calculate the reciprocal of x (1/x).
/// \return double
///         The reciprocal of x.
/// \param   double x
///         The value you want the reciprocal of.
inline double reciprocal( double x)
{
    return 1.0 / x;
}

/** inline function **/
/** floor           **/
//! Calculate the floor of x.
/// \return int32_t
///         The floor of x.
/// \param   float x
///         The value you want the floor of.
inline int32_t floor( float x)
{
    return (int32_t)floorf(x);
}

/** inline function **/
/** ceil            **/
//! Calculate the ceiling of x.
/// \return int32_t
///         The ceiling of x.
/// \param   float x
///         The value you want the ceiling of.
inline int32_t ceil( float x)
{
    return (int32_t)ceilf(x);
}

/** inline function **/
/** ceil            **/
//! Returns the fraction of x.
/// \return int32_t
///         The fraction of x.
/// \param   float x
///         The value you want the fraction of.
inline float fract( float x)
{
    return x - floorf(x);
}

//! A 2d vector template class.
template<class T>
class Vector2
{
public:
    //! Default constructor
    /*!
     * This creates the null-vector.
     */
    Vector2() :
        X(0),
        Y(0)
    {
        //
    }

    //! Constructor
    /*!
     * \param[in] x The x-coord of the vector.
     * \param[in] y The y-coord of the vector.
     */
    Vector2(T x, T y) :
        X(x),
        Y(y)
    {
        //
    }

    //! Constructor
    /*!
     * \param[in] n The size that will be used for both the vector components.
     */
    explicit Vector2(T n) :
        X(n),
        Y(n)
    {
        //
    }

    //! Constructor
    /*!
     * \param[in] other The vector you want to copy.
     */
    Vector2(const Vector2<T>& other) :
        X(other.X),
        Y(other.Y)
    {
        //
    }

    //! - operator
    /*!
     * Returns a vector with the sign of both components inverted.
     * \return The inverted vector.
     */
    Vector2<T> operator-(void) const
    {
        return Vector2<T>(-X, -Y);
    }

    //! = operator
    /*!
     * Copies a vector.
     * \return A reference to this vector.
     * \param[in] other The vector you want to copy.
     */
    Vector2<T>& operator=(const Vector2<T>& other)
    {
        X = other.X;
        Y = other.Y;
        return *this;
    }

    //! + operator
    /*!
     * Adds 2 vectors and returns the resulting one.
     * \return The sum of this vector with the other one.
     * \param[in] other The vector you want to add.
     */
    Vector2<T> operator+(const Vector2<T>& other) const
    {
        return Vector2<T>(X + other.X, Y + other.Y);
    }

    //! += operator
    /*!
     * Adds a vector to this one.
     * \return A reference to this vector after the addition.
     * \param[in] other The vector you want to add.
     */
    Vector2<T>& operator+=(const Vector2<T>& other)
    {
        X+=other.X;
        Y+=other.Y;
        return *this;
    }

    //! + operator
    /*!
     * Adds a vector with a value and returns the result.
     * \return The sum of this vector with the given value.
     * \param[in] v The value you want to add.
     */
    Vector2<T> operator+(T v) const
    {
        return Vector2<T>(X + v, Y + v);
    }

    //! += operator
    /*!
     * Adds a value to this vector.
     * \return A reference to this vector after the addition.
     * \param[in] v The value you want to add.
     */
    Vector2<T>& operator+=(T v)
    {
        X+=v;
        Y+=v;
        return *this;
    }

    //! - operator
    /*!
     * Subtracts 2 vectors and returns the resulting one.
     * \return The difference of this vector with the other one.
     * \param[in] other The vector you want to subtract with.
     */
    Vector2<T> operator-(const Vector2<T>& other) const
    {
        return Vector2<T>(X - other.X, Y - other.Y);
    }

    //! -= operator
    /*!
     * Subtracts a vector from this one.
     * \return A reference to this vector after the subtraction.
     * \param[in] other The vector you want to subtract with.
     */
    Vector2<T>& operator-=(const Vector2<T>& other)
    {
        X-=other.X;
        Y-=other.Y;
        return *this;
    }

    //! - operator
    /*!
     * Subtract a vector with a value and returns the result.
     * \return The difference of this vector with the given value.
     * \param[in] v The value you want to subtract with.
     */
    Vector2<T> operator-(T v) const
    {
        return Vector2<T>(X - v, Y - v);
    }

    //! -= operator
    /*!
     * Subtract a value to this vector.
     * \return A reference to this vector after the subtraction.
     * \param[in] v The value you want to subtract with.
     */
    Vector2<T>& operator-=(T v)
    {
        X-=v;
        Y-=v;
        return *this;
    }

    //! * operator
    /*!
     * Multiplies 2 vectors and returns the resulting one.
     * \return The product of this vector with the other one.
     * \param[in] other The vector you want to multiply with.
     */
    Vector2<T> operator*(const Vector2<T>& other) const
    {
        return Vector2<T>(X * other.X, Y * other.Y);
    }

    //! *= operator
    /*!
     * Multiply a vector with this one.
     * \return A reference to this vector after the multiplication.
     * \param[in] other The vector you want to multiply with.
     */
    Vector2<T>& operator*=(const Vector2<T>& other)
    {
        X*=other.X;
        Y*=other.Y;
        return *this;
    }

    //! * operator
    /*!
     * Multiply a vector with a value and returns the result.
     * \return The product of this vector with the given value.
     * \param[in] v The value you want to multiply with.
     */
    Vector2<T> operator*(T v) const
    {
        return Vector2<T>(X * v, Y * v);
    }

    //! *= operator
    /*!
     * Multiply a value to this vector.
     * \return A reference to this vector after the multiplication.
     * \param[in] v The value you want to multiply with.
     */
    Vector2<T>& operator*=(T v)
    {
        X*=v;
        Y*=v;
        return *this;
    }

    //! / operator
    /*!
     * Divides 2 vectors and returns the resulting one.
     * \return The quotient of this vector with the other one.
     * \param[in] other The vector you want to divide with.
     */
    Vector2<T> operator/(const Vector2<T>& other) const
    {
        return Vector2<T>(X / other.X, Y / other.Y);
    }

    //! /= operator
    /*!
     * Divide this vector with another one.
     * \return A reference to this vector after the division.
     * \param[in] other The vector you want to divide with.
     */
    Vector2<T>& operator/=(const Vector2<T>& other)
    {
        X/=other.X;
        Y/=other.Y;
        return *this;
    }

    //! / operator
    /*!
     * Divides a vector with a value and returns the result.
     * \return The quotient of this vector with the given value.
     * \param[in] v The value you want to divide with.
     */
    Vector2<T> operator/(T v) const
    {
        return Vector2<T>(X / v, Y / v);
    }

    //! /= operator
    /*!
     * Divide this vector with a value.
     * \return A reference to this vector after the division.
     * \param[in] v The value you want to divide with.
     */
    Vector2<T>& operator/=(T v)
    {
        X/=v;
        Y/=v;
        return *this;
    }

    //! <= operator
    /*!
     * \return True if this vector is smaller than or equal to the other vector.
     * \param[in] other The vector you want to perform the check with.
     */
    bool operator<=(const Vector2<T>& other) const
    {
        return (X<other.X || Myth::Math::equals(X, other.X)) || (Myth::Math::equals(X, other.X) && (Y<other.Y || Myth::Math::equals(Y, other.Y)));
    }

    //! >= operator
    /*!
     * \return True of this vector is greater than or equal to the other vector.
     * \param[in] other The vector you want to perform the check with.
     */
    bool operator>=(const Vector2<T>& other) const
    {
        return (X>other.X || Myth::Math::equals(X, other.X)) || (Myth::Math::equals(X, other.X) && (Y>other.Y || Myth::Math::equals(Y, other.Y)));
    }

    //! < operator
    /*!
     * \return True if this vector is smaller than the other vector.
     * \param[in] other The vector you want to perform the check with.
     */
    bool operator<(const Vector2<T>& other) const
    {
        return (X<other.X && !Myth::Math::equals(X, other.X)) || (Myth::Math::equals(X, other.X) && Y<other.Y && !Myth::Math::equals(Y, other.Y));
    }

    //! > operator
    /*!
     * \return True if this vector is greater than the other vector.
     * \param[in] other The vector you want to perform the check with.
     */
    bool operator>(const Vector2<T>& other) const
    {
        return (X>other.X && !Myth::Math::equals(X, other.X)) || (Myth::Math::equals(X, other.X) && Y>other.Y && !Myth::Math::equals(Y, other.Y));
    }

    //! == operator
    /*!
     * \return True if the vectors are equal, false otherwise.
     * \param[in] other The vector you want to perform the check with.
     */
    bool operator==(const Vector2<T>& other) const
    {
        return equals(other);
    }

    //! != operator
    /*!
     * \return True if the vectors are different, false otherwise.
     * \param[in] other The vector you want to perform the check with.
     */
    bool operator!=(const Vector2<T>& other) const
    {
        return !equals(other);
    }

    //! Checks if this vector equals the other one.
    /*!
     * \return True if the vectors are equal, false otherwise.
     * \param[in] other The vector you want to perform the check with.
     */
    bool equals(const Vector2<T>& other) const
    {
        return Myth::Math::equals(X, other.X) && Myth::Math::equals(Y, other.Y);
    }

    //! Sets both coordinate components
    /*!
     * \return A reference to this vector after the coords were set.
     * \param[in] x The new x-coord.
     * \param[in] y The new y-coord.
     */
    Vector2<T>& set(T x, T y)
    {
        X = x;
        Y = y;
        return *this;
    }

    //! Sets the components of this vector to match another ones.
    /*!
     * \return A reference to this vector after the coords were set.
     * \param[in] other The vector you want to copy its coordinates from.
     */
    Vector2<T>& set(const Vector2<T>& other)
    {
        X = other.X;
        Y = other.Y;
        return *this;
    }

    //! Returns the euclidean length of the vector.
    /*!
     * \return The length of this vector.
     */
    T get_length(void) const
    {
        return Myth::Math::squareroot(X*X + Y*Y);
    }

    //! Returns the squared euclidean length of the vector.
    /*!
     * \return The squared length of this vector.
     */
    T get_length_sq(void) const
    {
        return X*X + Y*Y;
    }

    //! Returns the dot product of this vector with another.
    /*!
     * \return The dot product of this vector with another.
     * \param[in] other The vector to calculate the dot product with.
     */
    T dot_product(const Vector2<T>& other) const
    {
        return X*other.X + Y*other.Y;
    }

    //! Returns the euclidean distance from another vector.
    /*!
     * \return T The distance between the two vectors.
     * \param[in] other The vector to measure from.
     */
    T get_distance_from(const Vector2<T>& other) const
    {
        return Vector2<T>(X - other.X, Y - other.Y).get_length();
    }

    //! Returns the squared euclidean distance from another vector.
    /*!
     * \return The squared distance between the two vectors.
     * \param[in] other The vector to measure from.
     */
    T get_distance_from_sq(const Vector2<T>& other) const
    {
        return Vector2<T>(X - other.X, Y - other.Y).get_length_sq();
    }

    //! Rotates this vector counter-clockwise around a center by an amount of degrees.
    /*!
     * \return A reference to this vector after the rotation.
     * \param[in] degrees The degrees you want to turn the vector.
     * \param[in] center The center of rotation.
     */
    Vector2<T>& rotate_by(double degrees, const Vector2<T>& center = Vector2<T>())
    {
        degrees *= Myth::Math::DEGTORAD64;
        const double cs = cos(degrees);
        const double sn = sin(degrees);

        X -= center.X;
        Y -= center.Y;

        set((T)(X*cs - Y*sn), (T)(X*sn + Y*cs));

        X += center.X;
        Y += center.Y;
        return *this;
    }

    //! Normalizes this vector.
    /*!
     * \return A reference to this vector after normalization.
     */
    Vector2<T>& normalize(void)
    {
        float length = (float)(X*X + Y*Y);
        if(Myth::Math::equals(length, 0.f))
            return *this;
        length = Myth::Math::reciprocal_squareroot(length);
        X = (T)(X * length);
        Y = (T)(Y * length);
        return *this;
    }

    //! Returns the angle of this vector in degrees in the trigonometric sense. 0 is 3 o'clock, value increases counter-clockwise.
    /*!
     * \return The angle of this vector in degrees.
     */
    double get_angle_trig(void) const
    {
        if(Y == 0)
            return X < 0 ? 180 : 0;
        else if(X == 0)
            return Y < 0 ? 270 : 90;

        if(Y > 0)
            if(X > 0)
                return atan(Y/X) * Myth::Math::RADTODEG64;
            else
                return 180.0-atan(Y/-X) * Myth::Math::RADTODEG64;
        else if(X > 0)
            return 360.0-atan(-Y/X) * Myth::Math::RADTODEG64;
        else
            return 180.0+atan(-Y/-X) * Myth::Math::RADTODEG64;
    }

    //! Returns the angle of this vector in degrees in the counter-trigonometric sense.
    /*!
     * \return The angle of this vector in degrees.
     */
    double get_angle(void) const
    {
        if(Y == 0)
            return X < 0 ? 180 : 0;
        else if(X == 0)
            return Y < 0 ? 90 : 270;

        double tmp = Y / Myth::Math::squareroot((double)(X*X + Y*Y));
        tmp = atan(Myth::Math::squareroot(1 - tmp*tmp) / tmp) * Myth::Math::RADTODEG64;

        if(X>0 && Y>0)
            return tmp + 270;
        else if(X>0 && Y<0)
            return tmp + 90;
        else if(X<0 && Y<0)
            return 90 - tmp;
        else if(X<0 && Y>0)
            return 270 - tmp;

        return tmp;
    }

    //! Calculates the angle between this vector and another one in degrees.
    /// \return double
    ///         The angle in degrees between 0 and 90.
    /// \param    const Vector2<T>& b
    ///         The other vector to calculate the angle with.
    double get_angle_with(const Vector2<T>& b) const
    {
        double tmp = X*b.X + Y*b.Y;

        if(tmp == 0.0)
            return 90.0;

        tmp = tmp / Myth::Math::squareroot((double)((X*X + Y*Y) * (b.X*b.X + b.Y*b.Y)));
        if(tmp < 0.0)
            tmp = -tmp;

        return atan(sqrt(1 - tmp*tmp) / tmp) * Myth::Math::RADTODEG64;
    }

    //! Returns if this vector is between two other points.
    /// \return bool
    ///         True if this vector is on the line between the other points.
    /// \param    const Vector2<T>& begin
    ///         The first vector of the line.
    /// \param    const Vector2<T>& end
    ///         The second vector of the line.
    bool is_between_points(const Vector2<T>& begin, const Vector2<T>& end) const
    {
        if(begin.X != end.X)
            return ((begin.X <= X && X <= end.X) || (begin.X >= X && X >= end.X));
        else
            return ((begin.Y <= Y && Y <= end.Y) || (begin.Y >= Y && Y >= end.Y));
    }

    //! Returns the interpolated vector between this vector and another vector.
    /// \return Vector2<T>
    ///         The interpolated vector.
    /// \param    const Vector2<T>& other
    ///         The other vector to interpolate with.
    /// \param    double d
    ///         The factor of interpolation, 0 = other vector, 1 = this vector.
    Vector2<T> get_interpolated(const Vector2<T>& other, double d) const
    {
        double inv = 1.0f - d;
        return Vector2<T>((T)(other.X*inv + X*d), (T)(other.Y*inv + Y*d));
    }

    //! Returns the quadratically interpolated vector between this and two other vectors.
    /// \return Vector2<T>
    ///         The quadratically interpolated vector.
    /// \param    const Vector2<T>& v2
    ///         The second vector to interpolate with.
    /// \param    const Vector2<T>& v3
    ///         The thirs vector to interpolate with.
    /// \param    double d
    ///         The factor of interpolation, 0 = this vector, 1 = 3th vector.
    Vector2<T> get_interpolated_quadratic(const Vector2<T>& v2, const Vector2<T>& v3, double d) const
    {
        const double inv = 1.0f - d;
        const double mul0 = inv * inv;
        const double mul1 = 2.0f * d * inv;
        const double mul2 = d * d;

        return Vector2<T> ((T)(X * mul0 + v2.X * mul1 + v3.X * mul2),
                           (T)(Y * mul0 + v2.Y * mul1 + v3.Y * mul2));
    }

    //! Sets this vector to the linearly interpolated vector between a and b.
    /// \return Vector2<T>&
    ///         A reference to this vector after the interpolation.
    /// \param    const Vector2<T>& a
    ///         The first vector to interpolate between.
    /// \param    const Vector2<T>& b
    ///         The second vector to interpolate between.
    /// \param    double d
    ///         The factor of interpolation, 0 = b, 1 = a.
    Vector2<T>& interpolate(const Vector2<T>& a, const Vector2<T>& b, double d)
    {
        X = (T)((double)b.X + ((a.X - b.X) * d));
        Y = (T)((double)b.Y + ((a.Y - b.Y) * d));
        return *this;
    }

    //! Returns the optimal size according to some properties.
    /// \return Vector2<T>
    ///         The optimal size under the given constraints.
    /// \param    bool require_power_of_two
    ///         If true forces the result to use only powers of two as values.
    /// \param    bool require_square
    ///         If true forces the width to be the same as the height.
    /// \param    bool larger
    ///         If true the result will be larger than the given dimension.
    /// \param    uint32_t max_value
    ///         The maximum size.
    Vector2<T> get_optimal_size(bool require_power_of_two = true, bool require_square = false, bool larger = true, uint32_t max_value = 0) const
    {
        uint32_t i=1;
        uint32_t j=1;
        if(require_power_of_two)
        {
            while(i<(uint32_t)X)
                i<<=1;
            if(!larger && i!=1 && i!=(uint32_t)X)
                i>>=1;
            while(j<(uint32_t)Y)
                j<<=1;
            if(!larger && j!=1 && j!=(uint32_t)Y)
                j>>=1;
        }
        else
        {
            i=(uint32_t)X;
            j=(uint32_t)Y;
        }

        if(require_square)
        {
            if((larger && (i>j)) || (!larger && (i<j)))
                j=i;
            else
                i=j;
        }

        if(max_value > 0 && i > max_value)
            i = max_value;

        if(max_value > 0 && j > max_value)
            j = max_value;

        return Vector2<T>((T)i,(T)j);
    }

    //! X coordinate of vector.
    T X;
    //! Y coordinate of vector.
    T Y;
};

//! A floating point 2d vector.
typedef Vector2<float> Vector2f;
//! An integer 2d vector.
typedef Vector2<int32_t> Vector2i;
//! An unsigned integer 2d vector.
typedef Vector2<uint32_t> Vector2u;

//! Multiplies a vector with a scalar.
/// \return Vector2<T>
///         The mulitplied vector.
/// \param    S scalar
///         The scalar to multiply with.
/// \param     const Vector2<T>& vector
///         The vector to be multiplied.
template<class S, class T>
Vector2<T> operator*(S scalar, const Vector2<T>& vector)
{
    return vector*scalar;
}

/** class   **/
/** Vector3 **/
//! A 3d vector template class.
template <class T>
class Vector3
{
public:
    /** default constructor **/
    Vector3() :
        X(0),
        Y(0),
        Z(0)
    {
        //
    }

    /** constructor **/
    /// \param    T x
    ///         The x-coord of the vector.
    /// \param    T y
    ///         The y-coord of the vector.
    /// \param    T z
    ///         The y-coord of the vector.
    Vector3(T x,
            T y,
            T z) :
        X(x),
        Y(y),
        Z(z)
    {
        //
    }

    /** explicit constructor **/
    /// \param    T n
    ///         The value you want for both the x- and the y-coord.
    explicit Vector3(T n) :
        X(n),
        Y(n),
        Z(n)
    {
        //
    }

    /** constructor **/
    /// \param    const Vector3<T>& other
    ///         The vector you want to copy.
    Vector3(const Vector3<T>& other) :
        X(other.X),
        Y(other.Y),
        Z(other.Z)
    {
        //
    }

    /** const operator **/
    /** -              **/
    //! Returns the inverted vector.
    /// \return Vector3<T>
    ///         The inverted vector.
    Vector3<T> operator-(void) const
    {
        return Vector3<T>(-X, -Y, -Z);
    }

    /** operator **/
    /** =        **/
    //! Copies a vector.
    /// \return Vector3<T>&
    ///         A reference this vector, after copying.
    /// \param    const Vector3<T>& other
    ///         The vector you want to copy.
    Vector3<T>& operator=(const Vector3<T>& other)
    {
        X = other.X;
        Y = other.Y;
        Z = other.Z;
        return *this;
    }

    /** const operator **/
    /** +              **/
    //! Adds 2 vectors.
    /// \return Vector3<T>
    ///         The addition between this vector and the other one.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to add.
    Vector3<T> operator+(const Vector3<T>& other) const
    {
        return Vector3<T>(X + other.X, Y + other.Y, Z + other.Z);
    }

    /** operator **/
    /** +=       **/
    //! Adds a vector to this one.
    /// \return Vector3<T>&
    ///         A reference to this vector after adding.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to add to this one.
    Vector3<T>& operator+=(const Vector3<T>& other)
    {
        X+=other.X;
        Y+=other.Y;
        Z+=other.Z;
        return *this;
    }

    /** const operator **/
    /** +              **/
    //! Adds a vector with a number.
    /// \return Vector3<T>
    ///         The addition between the number and this vector.
    /// \param    T v
    ///         The number you want to add with this vector.
    Vector3<T> operator+(T val) const
    {
        return Vector3<T>(X + val, Y + val, Z + val);
    }

    /** operator **/
    /** +=       **/
    //! Adds a number to this vector.
    /// \return Vector3<T>&
    ///         A reference to this vector after adding.
    /// \param    T v
    ///         The number you want to add to this vector.
    Vector3<T>& operator+=(T val)
    {
        X+=val;
        Y+=val;
        Z+=val;
        return *this;
    }

    /** const operator **/
    /** -              **/
    //! Subtracts 2 vectors.
    /// \return Vector3<T>
    ///         The difference between this vector and the other one.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to subtract.
    Vector3<T> operator-(const Vector3<T>& other) const
    {
        return Vector3<T>(X - other.X, Y - other.Y, Z - other.Z);
    }

    /** operator **/
    /** -=       **/
    //! Subtracts a vector to this one.
    /// \return Vector3<T>&
    ///         A reference to this vector after subtraction.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to subtract to this one.
    Vector3<T>& operator-=(const Vector3<T>& other)
    {
        X-=other.X;
        Y-=other.Y;
        Z-=other.Z;
        return *this;
    }

    /** const operator **/
    /** -              **/
    //! Subtracts a vector with a number.
    /// \return Vector3<T>
    ///         The difference between the number and this vector.
    /// \param    T v
    ///         The number you want to subtract with this vector.
    Vector3<T> operator-(T val) const
    {
        return Vector3<T>(X - val, Y - val, Z - val);
    }

    /** operator **/
    /** -=       **/
    //! Subtracts a number to this vector.
    /// \return Vector3<T>&
    ///         A reference to this vector after subtraction.
    /// \param    T v
    ///         The number you want to subtract to this vector.
    Vector3<T>& operator-=(T val)
    {
        X-=val;
        Y-=val;
        Z-=val;
        return *this;
    }

    /** const operator **/
    /** *              **/
    //! Multiplies 2 vectors.
    /// \return Vector3<T>
    ///         The product between this vector and the other one.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to multiply.
    Vector3<T> operator*(const Vector3<T>& other) const
    {
        return Vector3<T>(X * other.X, Y * other.Y, Z * other.Z);
    }

    /** operator **/
    /** *=       **/
    //! Multiplies a vector to this one.
    /// \return Vector3<T>&
    ///         A reference to this vector after multiplication.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to multiply to this one.
    Vector3<T>& operator*=(const Vector3<T>& other)
    {
        X*=other.X;
        Y*=other.Y;
        Z*=other.Z;
        return *this;
    }

    /** const operator **/
    /** *              **/
    //! Multiplies a vector with a number.
    /// \return Vector3<T>
    ///         The product between the number and this vector.
    /// \param    T v
    ///         The number you want to multiply with this vector.
    Vector3<T> operator*(T v) const
    {
        return Vector3<T>(X * v, Y * v, Z * v);
    }

    /** operator **/
    /** *=       **/
    //! Multiplies a number to this vector.
    /// \return Vector3<T>&
    ///         A reference to this vector after multiplication.
    /// \param    T v
    ///         The number you want to multiply to this vector.
    Vector3<T>& operator*=(T v)
    {
        X*=v;
        Y*=v;
        Z*=v;
        return *this;
    }

    /** const operator **/
    /** /              **/
    //! Divides 2 vectors.
    /// \return Vector3<T>
    ///         The quotient between this vector and the other one.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to divide.
    Vector3<T> operator/(const Vector3<T>& other) const
    {
        return Vector3<T>(X / other.X, Y / other.Y, Z / other.Z);
    }

    /** operator **/
    /** /=       **/
    //! Divides a vector to this one.
    /// \return Vector3<T>&
    ///         A reference to this vector after division.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to divide to this one.
    Vector3<T>& operator/=(const Vector3<T>& other)
    {
        X/=other.X;
        Y/=other.Y;
        Z/=other.Z;
        return *this;
    }

    /** const operator **/
    /** /              **/
    //! Divides a vector with a number.
    /// \return Vector3<T>
    ///         The quotient between the number and this vector.
    /// \param    T v
    ///         The number you want to divide with this vector.
    Vector3<T> operator/(T v) const
    {
        T i=(T)1.0/v;
        return Vector3<T>(X * i, Y * i, Z * i);
    }

    /** operator **/
    /** /=       **/
    //! Divides a number to this vector.
    /// \return Vector3<T>&
    ///         A reference to this vector after division.
    /// \param    T v
    ///         The number you want to divide to this vector.
    Vector3<T>& operator/=(T v)
    {
        T i=(T)1.0/v;
        X*=i;
        Y*=i;
        Z*=i;
        return *this;
    }

    /** const operator **/
    /** <=             **/
    //! Sort in order X, Y. Equality with rounding tolerance.
    /// \return bool
    ///         True if this vector is smaller than or equal to the other vector.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    bool operator<=(const Vector3<T>& other) const
    {
        return	(X<other.X || Myth::Math::equals(X, other.X)) ||
                (Myth::Math::equals(X, other.X) && (Y<other.Y || Myth::Math::equals(Y, other.Y))) ||
                (Myth::Math::equals(X, other.X) && Myth::Math::equals(Y, other.Y) && (Z<other.Z || Myth::Math::equals(Z, other.Z)));
    }

    /** const operator **/
    /** >=             **/
    //! Sort in order X, Y. Equality with rounding tolerance.
    /// \return bool
    ///         True if this vector is greater than or equal to the other vector.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    bool operator>=(const Vector3<T>& other) const
    {
        return	(X>other.X || Myth::Math::equals(X, other.X)) ||
                (Myth::Math::equals(X, other.X) && (Y>other.Y || Myth::Math::equals(Y, other.Y))) ||
                (Myth::Math::equals(X, other.X) && Myth::Math::equals(Y, other.Y) && (Z>other.Z || Myth::Math::equals(Z, other.Z)));
    }

    /** const operator **/
    /** <              **/
    //! Sort in order X, Y. Difference with rounding tolerance.
    /// \return bool
    ///         True if this vector is smaller than the other vector.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    bool operator<(const Vector3<T>& other) const
    {
        return	(X<other.X && !Myth::Math::equals(X, other.X)) ||
                (Myth::Math::equals(X, other.X) && Y<other.Y && !Myth::Math::equals(Y, other.Y)) ||
                (Myth::Math::equals(X, other.X) && Myth::Math::equals(Y, other.Y) && Z<other.Z && !Myth::Math::equals(Z, other.Z));
    }

    /** const operator **/
    /** >              **/
    //! Sort in order X, Y. Difference with rounding tolerance.
    /// \return bool
    ///         True if this vector is greater than the other vector.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    bool operator>(const Vector3<T>& other) const
    {
        return	(X>other.X && !Myth::Math::equals(X, other.X)) ||
                (Myth::Math::equals(X, other.X) && Y>other.Y && !Myth::Math::equals(Y, other.Y)) ||
                (Myth::Math::equals(X, other.X) && Myth::Math::equals(Y, other.Y) && Z>other.Z && !Myth::Math::equals(Z, other.Z));
    }

    /** const operator **/
    /** ==             **/
    //! Checks if this vector equals the other one.
    /// \return bool
    ///         True if the vectors are equal, false otherwise.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    bool operator==(const Vector3<T>& other) const
    {
        return this->equals(other);
    }

    /** const operator **/
    /** !=             **/
    //! Checks if this vector is different from the other one.
    /// \return bool
    ///         True if the vectors are different, false otherwise.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    bool operator!=(const Vector3<T>& other) const
    {
        return !this->equals(other);
    }

    /** const function **/
    /** equals         **/
    //! Checks if this vector equals the other one.
    /// \return bool
    ///         True if the vectors are equal, false otherwise.
    /// \param    const Vector3<T>& other
    ///         The other vector you want to check with.
    /// \param    const T tolerance
    ///         The tolerance you want to use.
    bool equals(const Vector3<T>& other,
                const T tolerance = (T)Myth::Math::ROUNDING_ERROR_32) const
    {
        return Myth::Math::equals(X, other.X, tolerance) &&
               Myth::Math::equals(Y, other.Y, tolerance) &&
               Myth::Math::equals(Z, other.Z, tolerance);
    }

    /** function **/
    /** set      **/
    //! Sets the coordinates.
    /// \return Vector3<T>&
    ///         A reference to this vector after the coords were set.
    /// \param    T x
    ///         The new x-coord.
    /// \param    T y
    ///         The new y-coord.
    /// \param    T z
    ///         The new z-coord.
    Vector3<T>& set(T x,
                    T y,
                    T z)
    {
        X = x;
        Y = y;
        Z = z;
        return *this;
    }

    /** function **/
    /** set      **/
    //! Sets the coordinates.
    /// \return Vector3<T>&
    ///         A reference to this vector after the coords were set.
    /// \param    const Vector3<T>& p
    ///         The new coordinates.
    Vector3<T>& set(const Vector3<T>& p)
    {
        X = p.X;
        Y = p.Y;
        Z = p.Z;
        return *this;
    }

    /** const function **/
    /** get_length     **/
    //! Returns the length of this vector.
    /// \return T
    ///         The length of this vector.
    /// \param  void
    T get_length(void) const
    {
        return Myth::Math::squareroot(X*X + Y*Y + Z*Z);
    }

    /** const function **/
    /** get_length_sq  **/
    //! Returns the squared length of this vector, is much faster than get_length().
    /// \return T
    ///         The squared length of this vector.
    T get_length_sq(void) const
    {
        return X*X + Y*Y + Z*Z;
    }

    /** const function **/
    /** dot_product    **/
    //! Returns the dot product of this vector with another.
    /// \return T
    ///         The dot product of this vector with another.
    /// \param    const Vector3<T>& other
    ///         The other vector to take the dot product with.
    T dot_product(const Vector3<T>& other) const
    {
        return X*other.X + Y*other.Y + Z*other.Z;
    }

    /** const function    **/
    /** get_distance_from **/
    //! Returns the distance from the other point.
    /// \return T
    ///         The distance from the other point.
    /// \param    const Vector3<T>& other
    ///         The other vector to measure from.
    T get_distance_from(const Vector3<T>& other) const
    {
        return Vector3<T>(X - other.X, Y - other.Y, Z - other.Z).get_length();
    }

    /** const function       **/
    /** get_distance_from_sq **/
    //! Returns the squared distance from the other point.
    /// \return T
    ///         The squared distance from the other point.
    /// \param    const Vector3<T>& other
    ///         The other vector to measure from.
    T get_distance_from_sq(const Vector3<T>& other) const
    {
        return Vector3<T>(X - other.X, Y - other.Y, Z - other.Z).get_length_sq();
    }

    /** const function **/
    /** cross_product  **/
    //! Returns the cross product of this vector with another.
    /// \return T
    ///         The cross product of this vector with another.
    /// \param    const Vector3<T>& other
    ///         The other vector to take the cross product with.
    Vector3<T> cross_product(const Vector3<T>& p) const
    {
        return Vector3<T>(Y * p.Z - Z * p.Y, Z * p.X - X * p.Z, X * p.Y - Y * p.X);
    }

    /** const function    **/
    /** is_between_points **/
    //! Returns if this vector is between two other points.
    /// \return bool
    ///         True if this vector is on the line between the other points.
    /// \param    const Vector3<T>& begin
    ///         The first vector of the line.
    /// \param    const Vector3<T>& end
    ///         The second vector of the line.
    bool is_between_points(const Vector3<T>& begin,
                           const Vector3<T>& end) const
    {
        const T f = (end - begin).get_length_sq();
        return get_distance_from_sq(begin) <= f &&
               get_distance_from_sq(end) <= f;
    }

    /** function  **/
    /** normalize **/
    //! Normalizes this vector.
    /// \return Vector3<T>&
    ///         A reference to this vector, after normalization.
    /// \param  void
    Vector3<T>& normalize(void)
    {
        double length = X*X + Y*Y + Z*Z;
        if(Myth::Math::equals(length, 0.0))  // this check isn't an optimization but prevents getting NAN in the sqrt.
            return *this;
        length = Myth::Math::reciprocal_squareroot(length);

        X = (T)(X * length);
        Y = (T)(Y * length);
        Z = (T)(Z * length);
        return *this;
    }

    /** function   **/
    /** set_length **/
    //! Sets the length of the vector to a new value
    /// \return Vector3<T>&
    ///         A reference to this vector, after resizing.
    /// \param    T newlength
    ///         The new length.
    Vector3<T>& set_length(T newlength)
    {
        normalize();
        return (*this *= newlength);
    }

    /** function **/
    /** invert   **/
    //! Inverts this vector.
    /// \return Vector3<T>&
    ///         A reference to this vector, after inversion.
    /// \param  void
    Vector3<T>& invert(void)
    {
        X *= -1;
        Y *= -1;
        Z *= -1;
        return *this;
    }

    /** function     **/
    /** rotate_xz_by **/
    //! Rotates the vector by a specified number of degrees around the Y axis and the specified center.
    /// \return void
    /// \param    double degrees
    ///         The number of degrees to rotate around the Y-axis.
    /// \param    const Vector3<T>& center
    ///         The center of rotation.
    void rotate_xz_by(double degrees,
                      const Vector3<T>& center=Vector3<T>())
    {
        degrees *= Myth::Math::DEGTORAD64;
        double cs = cos(degrees);
        double sn = sin(degrees);
        X -= center.X;
        Z -= center.Z;
        set((T)(X*cs - Z*sn), Y, (T)(X*sn + Z*cs));
        X += center.X;
        Z += center.Z;
    }

    /** function     **/
    /** rotate_xy_by **/
    //! Rotates the vector by a specified number of degrees around the Z axis and the specified center.
    /// \return void
    /// \param    double degrees
    ///         The number of degrees to rotate around the Z-axis.
    /// \param    const Vector3<T>& center
    ///         The center of rotation.
    void rotate_xy_by(double degrees,
                      const Vector3<T>& center=Vector3<T>())
    {
        degrees *= Myth::Math::DEGTORAD64;
        double cs = cos(degrees);
        double sn = sin(degrees);
        X -= center.X;
        Y -= center.Y;
        set((T)(X*cs - Y*sn), (T)(X*sn + Y*cs), Z);
        X += center.X;
        Y += center.Y;
    }

    /** function     **/
    /** rotate_yz_by **/
    //! Rotates the vector by a specified number of degrees around the X axis and the specified center.
    /// \return void
    /// \param    double degrees
    ///         The number of degrees to rotate around the X-axis.
    /// \param    const Vector3<T>& center
    ///         The center of rotation.
    void rotate_yz_by(double degrees,
                      const   Vector3<T>& center=Vector3<T>())
    {
        degrees *= Myth::Math::DEGTORAD64;
        double cs = cos(degrees);
        double sn = sin(degrees);
        Z -= center.Z;
        Y -= center.Y;
        set(X, (T)(Y*cs - Z*sn), (T)(Y*sn + Z*cs));
        Z += center.Z;
        Y += center.Y;
    }

    /** const function   **/
    /** get_interpolated **/
    //! Returns the interpolated vector between this vector and another vector.
    /// \return Vector3<T>
    ///         The interpolated vector.
    /// \param    const Vector3<T>& other
    ///         The other vector to interpolate with.
    /// \param    double d
    ///         The factor of interpolation, 0 = other vector, 1 = this vector.
    Vector3<T> get_interpolated(const Vector3<T>& other,
                                double d) const
    {
        const double inv = 1.0 - d;
        return Vector3<T>((T)(other.X*inv + X*d), (T)(other.Y*inv + Y*d), (T)(other.Z*inv + Z*d));
    }

    /** const function             **/
    /** get_interpolated_quadratic **/
    //! Returns the quadratically interpolated vector between this and two other vectors.
    /// \return Vector3<T>
    ///         The quadratically interpolated vector.
    /// \param    const Vector3<T>& v2
    ///         The second vector to interpolate with.
    /// \param    const Vector3<T>& v3
    ///         The thirs vector to interpolate with.
    /// \param    double d
    ///         The factor of interpolation, 0 = this vector, 1 = 3th vector.
    Vector3<T> get_interpolated_quadratic(const Vector3<T>& v2,
                                          const Vector3<T>& v3,
                                          double d) const
    {
        // this*(1-d)*(1-d) + 2 * v2 * (1-d) + v3 * d * d;
        const double inv = (T) 1.0 - d;
        const double mul0 = inv * inv;
        const double mul1 = (T) 2.0 * d * inv;
        const double mul2 = d * d;

        return Vector3<T> ((T)(X * mul0 + v2.X * mul1 + v3.X * mul2),
                           (T)(Y * mul0 + v2.Y * mul1 + v3.Y * mul2),
                           (T)(Z * mul0 + v2.Z * mul1 + v3.Z * mul2));
    }

    /** function    **/
    /** interpolate **/
    //! Sets this vector to the linearly interpolated vector between a and b.
    /// \return Vector3<T>&
    ///         A reference to this vector after the interpolation.
    /// \param    const Vector3<T>& a
    ///         The first vector to interpolate between.
    /// \param    const Vector3<T>& b
    ///         The second vector to interpolate between.
    /// \param    double d
    ///         The factor of interpolation, 0 = b, 1 = a.
    Vector3<T>& interpolate(const Vector3<T>& a,
                            const Vector3<T>& b,
                            double d)
    {
        X = (T)((double)b.X + ((a.X - b.X) * d));
        Y = (T)((double)b.Y + ((a.Y - b.Y) * d));
        Z = (T)((double)b.Z + ((a.Z - b.Z) * d));
        return *this;
    }

    /** const function       **/
    /** get_horizontal_angle **/
    //! Returns the rotations that would make a (0,0,1) direction vector point in the same direction as this direction vector.
    /** Thanks to Arras on the Irrlicht forums for this method.  This utility method is very useful for
    orienting scene nodes towards specific targets.  For example, if this vector represents the difference
    between two scene nodes, then applying the result of getHorizontalAngle() to one scene node will point
    it at the other one.
    Example code:
    // Where target and seeker are of type ISceneNode*
    const Vector3f toTarget(target->getAbsolutePosition() - seeker->getAbsolutePosition());
    const Vector3f requiredRotation = toTarget.getHorizontalAngle();
    seeker->setRotation(requiredRotation); */
    /// \return Vector3<T>
    ///         A rotation vector containing the X and Y rotations that when applied to a +Z (e.g. 0, 0, 1) direction vector would make it point in the same direction as this vector.
    /// \param  void
    Vector3<T> get_horizontal_angle(void) const
    {
        Vector3<T> angle;

        const double tmp = (atan2((double)X, (double)Z) * Myth::Math::RADTODEG64);
        angle.Y = (T)tmp;

        if(angle.Y < 0)
            angle.Y += 360;
        if(angle.Y >= 360)
            angle.Y -= 360;

        const double z1 = Myth::Math::squareroot(X*X + Z*Z);

        angle.X = (T)(atan2((double)z1, (double)Y) * Myth::Math::RADTODEG64 - 90.0);

        if(angle.X < 0)
            angle.X += 360;
        if(angle.X >= 360)
            angle.X -= 360;

        return angle;
    }

    /** const function                  **/
    /** get_spherical_coordinate_angles **/
    //! Returns the Euler degrees for the point represented by this vector.
    /// \return Vector3<T>
    ///         The Euler degrees for the point represented by this vector.
    /// \param  void
    Vector3<T> get_spherical_coordinate_angles(void) const
    {
        Vector3<T> angle;
        const double length = X*X + Y*Y + Z*Z;

        if(length)
        {
            if(X!=0)
            {
                angle.Y = (T)(atan2((double)Z,(double)X) * Myth::Math::RADTODEG64);
            }
            else if(Z<0)
                angle.Y=180;

            angle.X = (T)(acos(Y * Myth::Math::reciprocal_squareroot(length)) * Myth::Math::RADTODEG64);
        }
        return angle;
    }

    /** const function        **/
    /** rotation_to_direction **/
    //! Returns a direction vector from this rotation vector.
    /// \return Vector3<T>
    ///         A direction vector from this rotation vector.
    /// \param    const Vector3<T>& forwards
    ///         A vector representing forwards, which will be rotated by this vector.
    Vector3<T> rotation_to_direction(const Vector3<T>& forwards = Vector3<T>(0, 0, 1)) const
    {
        const double cr = cos(Myth::Math::DEGTORAD64 * X);
        const double sr = sin(Myth::Math::DEGTORAD64 * X);
        const double cp = cos(Myth::Math::DEGTORAD64 * Y);
        const double sp = sin(Myth::Math::DEGTORAD64 * Y);
        const double cy = cos(Myth::Math::DEGTORAD64 * Z);
        const double sy = sin(Myth::Math::DEGTORAD64 * Z);

        const double srsp = sr*sp;
        const double crsp = cr*sp;

        const double pseudoMatrix[] =
        {
            (cp* cy), (cp* sy), (-sp),
            (srsp* cy-cr* sy), (srsp* sy+cr* cy), (sr* cp),
            (crsp* cy+sr* sy), (crsp* sy-sr* cy), (cr* cp)
        };

        return Vector3<T>(
                   (T)(forwards.X * pseudoMatrix[0] +
                       forwards.Y * pseudoMatrix[3] +
                       forwards.Z * pseudoMatrix[6]),
                   (T)(forwards.X * pseudoMatrix[1] +
                       forwards.Y * pseudoMatrix[4] +
                       forwards.Z * pseudoMatrix[7]),
                   (T)(forwards.X * pseudoMatrix[2] +
                       forwards.Y * pseudoMatrix[5] +
                       forwards.Z * pseudoMatrix[8]));
    }

    /** const function  **/
    /** get_as_4_values **/
    //! Fills an array of 4 values with the vector data.
    /// \return void
    /// \param    T* array
    ///         The aray you want to fill.
    void get_as_4_values(T* array) const
    {
        array[0] = X;
        array[1] = Y;
        array[2] = Z;
        array[3] = 0;
    }

    //! X coordinate of the vector
    T X;
    //! Y coordinate of the vector
    T Y;
    //! Z coordinate of the vector
    T Z;
};

/** const operator **/
/** /              **/
//! Divides a vector with a number.
/// \return Vector3<int32_t>
///         The quotient between the number and this vector.
/// \param    int32_t val
///         The number you want to divide with this vector.
template <>
inline Vector3<int32_t> Vector3<int32_t>::operator /(int32_t val) const
{
    return Vector3<int32_t>(X/val,Y/val,Z/val);
}

/** operator **/
/** /=       **/
//! Divides a number to this vector.
/// \return Vector3<int32_t>&
///         A reference to this vector after division.
/// \param    int32_t val
///         The number you want to divide to this vector.
template <>
inline Vector3<int32_t>& Vector3<int32_t>::operator /=(int32_t val)
{
    X/=val;
    Y/=val;
    Z/=val;
    return *this;
}

/** typedef  **/
/** Vector3f **/
//! A floating point 3d vector.
typedef Vector3<float> Vector3f;

/** typedef  **/
/** Vector3i **/
//! An integer 3d vector.
typedef Vector3<int32_t> Vector3i;

/** operator **/
/** *        **/
//! Multiplies a vector with a scalar.
/// \return Vector3<T>
///         The mulitplied vector.
/// \param    S scalar
///         The scalar to multiply with.
/// \param     const Vector3<T>& vector
///         The vector to be multiplied.
template<class S, class T>
Vector3<T> operator*(S scalar,   const Vector3<T>& vector)
{
    return vector*scalar;
}

/** class **/
/** Line2 **/
//! A 2d line template class.
template <class T>
class Line2
{
public:
    /** default constructor **/
    Line2() :
        mStart(0,0),
        mEnd(1,1)
    {
        //
    }

    /** constructor **/
    /// \param    T xa
    ///         The x-coord of the first point on the line.
    /// \param    T ya
    ///         The y-coord of the first point on the line.
    /// \param    T xb
    ///         The x-coord of the second point on the line.
    /// \param    T yb
    ///         The y-coord of the second point on the line.
    Line2(T xa,
          T ya,
          T xb,
          T yb) :
        mStart(xa, ya),
        mEnd(xb, yb)
    {
        //
    }

    /** constructor **/
    /// \param    const Vector2<T>& s
    ///         The first point on the line.
    /// \param    const Vector2<T>& e
    ///         The second point on the line.
    Line2(const Vector2<T>& s,
          const Vector2<T>& e) :
        mStart(s),
        mEnd(e)
    {
        //
    }

    /** constructor **/
    /// \param    const Line2<T>& other
    ///         The other line to copy.
    Line2(const Line2<T>& other) :
        mStart(other.mStart),
        mEnd(other.mEnd)
    {
        //
    }

    /** const operator **/
    /** +              **/
    //! Moves the line by the given vector.
    /// \return Line2<T>
    ///         The moved line.
    /// \param    const Vector2<T>& vec
    ///         The amount to move.
    Line2<T> operator+(const Vector2<T>& vec) const
    {
        return Line2<T>(mStart + vec, mEnd + vec);
    }

    /** operator **/
    /** +=       **/
    //! Moves this line with the given vector.
    /// \return Line2<T>&
    ///         A reference to this vector after the transformation.
    /// \param    const Vector2<T>& vec
    ///         The amount to move.
    Line2<T>& operator+=(const Vector2<T>& vec)
    {
        mStart += vec;
        mEnd += vec;
        return *this;
    }

    /** const operator **/
    /** -              **/
    //! Moves the line by the given vector.
    /// \return Line2<T>
    ///         The moved line.
    /// \param    const Vector2<T>& vec
    ///         The amount to move.
    Line2<T> operator-(const Vector2<T>& vec) const
    {
        return Line2<T>(mStart - vec, mEnd - vec);
    }

    /** operator **/
    /** -=       **/
    //! Moves this line with the given vector.
    /// \return Line2<T>&
    ///         A reference to this vector after the transformation.
    /// \param    const Vector2<T>& vec
    ///         The amount to move.
    Line2<T>& operator-=(const Vector2<T>& vec)
    {
        mStart -= vec;
        mEnd -= vec;
        return *this;
    }

    /** const operator **/
    /** ==             **/
    //! Checks if the two lines are the same.
    /// \return bool
    ///         True if the lines are the same, false otherwise.
    /// \param    const Line2<T>& other
    ///         The other line to check with.
    bool operator==(const Line2<T>& other) const
    {
        return (mStart==other.mStart && mEnd==other.mEnd) || (mEnd==other.mStart && mStart==other.mEnd);
    }

    /** const operator **/
    /** !=             **/
    //! Checks if the two lines are different.
    /// \return bool
    ///         True if the lines are different, false otherwise.
    /// \param    const Line2<T>& other
    ///         The other line to check with.
    bool operator!=(const Line2<T>& other) const
    {
        return !(mStart==other.mStart && mEnd==other.mEnd) || (mEnd==other.mStart && mStart==other.mEnd);
    }

    /** function **/
    /** set_line **/
    //! Set this line to new line going through the two points.
    /// \return void
    /// \param    T xa
    ///         The x-coord of the first point.
    /// \param    T ya
    ///         The y-coord of the first point.
    /// \param    T xb
    ///         The x-coord of the second point.
    /// \param    T yb
    ///         The y-coord of the second point.
    void set_line(T xa,
                  T ya,
                  T xb,
                  T yb)
    {
        mStart.set(xa, ya);
        mEnd.set(xb, yb);
    }

    /** function **/
    /** set_line **/
    //! Set this line to new line going through the two points.
    /// \return void
    /// \param    const Vector2<T>& start
    ///         The first point.
    /// \param    const Vector2<T>& end
    ///         The second point.
    void set_line(const Vector2<T>& start,
                  const Vector2<T>& end)
    {
        mStart.set(start);
        mEnd.set(end);
    }

    /** function **/
    /** set_line **/
    //! Copies a line.
    /// \return void
    /// \param    const Line2<T>& line
    ///         The line to copy.
    void set_line(const Line2<T>& line)
    {
        mStart.set(line.mStart);
        mEnd.set(line.mEnd);
    }

    /** const function **/
    /** get_length     **/
    //! Returns the length of this line.
    /// \return double
    ///         The length of this line.
    /// \param  void
    double get_length(void) const
    {
        return mStart.get_distance_from(mEnd);
    }

    /** const function **/
    /** get_length_sq  **/
    //! Returns the squared length of this line.
    /// \return T
    ///         The squared length of this line.
    /// \param  void
    T get_length_sq(void) const
    {
        return mStart.get_distance_from_sq(mEnd);
    }

    /** const function **/
    /** get_center     **/
    //! Returns the center of this line.
    /// \return Vector2<T>
    ///         The center of this line.
    /// \param  void
    Vector2<T> get_center(void) const
    {
        return (mStart + mEnd) * (T)0.5;
    }

    /** const function **/
    /** get_vector     **/
    //! Returns the vector of this line.
    /// \return Vector2<T>
    ///         The vector of this line.
    /// \param  void
    Vector2<T> get_vector(void) const
    {
        return Vector2<T>(mEnd.X - mStart.X, mEnd.Y - mStart.Y);
    }

    /** const function **/
    /** intersect_with **/
    //! Tests if this line intersects with another line.
    /// \return bool
    ///         True if there is an intersection, false otherwise.
    /// \param    const Line2<T>& l
    ///         The line to intersect with.
    /// \param    Vector2<T>& out
    ///         The location of the intersection will be written here, if needed.
    bool intersect_with(const Line2<T>& l,
                        Vector2<T>& out) const
    {
        // Uses the method given at:
        // http://local.wasp.uwa.edu.au/~pbourke/geometry/lineLine2/
        const float commonDenominator = (l.mEnd.Y - l.mStart.Y)*(mEnd.X - mStart.X) -
                                        (l.mEnd.X - l.mStart.X)*(mEnd.Y - mStart.Y);

        const float numeratorA = (l.mEnd.X - l.mStart.X)*(mStart.Y - l.mStart.Y) -
                                 (l.mEnd.Y - l.mStart.Y)*(mStart.X -l.mStart.X);

        const float numeratorB = (mEnd.X - mStart.X)*(mStart.Y - l.mStart.Y) -
                                 (mEnd.Y - mStart.Y)*(mStart.X -l.mStart.X);

        if(Myth::Math::equals(commonDenominator, 0.f))
        {
            // The lines are either coincident or parallel
            if(Myth::Math::equals(numeratorA, 0.f) && Myth::Math::equals(numeratorB, 0.f))
            {
                // Try and find a common endpoint
                if(l.mStart == mStart || l.mEnd == mStart)
                    out = mStart;
                else if(l.mEnd == mEnd || l.mStart == mEnd)
                    out = mEnd;
                else
                    // one line is contained in the other, so for lack of a better
                    // answer, pick the average of both lines
                    out = ((mStart + mEnd + l.mStart + l.mEnd) * 0.25f);

                return true; // coincident
            }

            return false; // parallel
        }

        // Get the point of intersection on this line, checking that
        // it is within the line segment.
        const float uA = numeratorA / commonDenominator;
        if(uA < 0.f || uA > 1.f)
            return false; // Outside the line segment

        const float uB = numeratorB / commonDenominator;
        if(uB < 0.f || uB > 1.f)
            return false; // Outside the line segment

        // Calculate the intersection point.
        out.X = mStart.X + uA * (mEnd.X - mStart.X);
        out.Y = mStart.Y + uA * (mEnd.Y - mStart.Y);
        return true;
    }

    /** const function  **/
    /** get_unit_vector **/
    //! Returns the unit vector of this line.
    /// \return Vector2<T>
    ///         The unit vector of this line.
    /// \param  void
    Vector2<T> get_unit_vector(void) const
    {
        T len = (T)(1.0 / get_length());
        return Vector2<T>((mEnd.X - mStart.X) * len, (mEnd.Y - mStart.Y) * len);
    }

    /** const function **/
    /** get_angle_with **/
    //! Returns the angle between this line and another line.
    /// \return double
    ///         The angle in degrees.
    /// \param    const Line2<T>& l
    ///         The other line to test with.
    double get_angle_with(const Line2<T>& l) const
    {
        Vector2<T> vect = get_vector();
        Vector2<T> vect2 = l.get_vector();
        return vect.get_angle_with(vect2);
    }

    /** const function        **/
    /** get_point_orientation **/
    //! Tells us if the given point lies to the left, right, or on the line.
    /// \return T
    ///         0 if the point is on the line, <0 if to the left, or >0 if to the right.
    /// \param    const Vector2<T>& point
    ///         The point to get the orientation from.
    T get_point_orientation(const Vector2<T>& point) const
    {
        return ((mEnd.X - mStart.X) * (point.Y - mStart.Y) -
                (point.X - mStart.X) * (mEnd.Y - mStart.Y));
    }

    /** const function   **/
    /** is_point_on_line **/
    //! Checks if the given point is a member of the line.
    /// \return bool
    ///         True if the point lies on the line, false otherwise.
    /// \param    const Vector2<T>& point
    ///         The point to check.
    bool is_point_on_line(const Vector2<T>& point) const
    {
        T d = get_point_orientation(point);
        return (d == 0 && point.is_between_points(mStart, mEnd));
    }

    /** const function                 **/
    /** is_point_between_start_and_end **/
    //! Checks if the given point is somewhere between the start and the end of the line.
    /// \return bool
    ///         True if the point lies between start and end.
    /// \param    const Vector2<T>& point
    ///         The point to check.
    bool is_point_between_start_and_end(const Vector2<T>& point) const
    {
        return point.is_between_points(mStart, mEnd);
    }

    /** const function    **/
    /** get_closest_point **/
    //! Returns the closest point on this line to a point.
    /// \return Vector2<T>
    ///         The closest point on this line to a point.
    /// \param    const Vector2<T>& point
    ///         The point to check with.
    Vector2<T> get_closest_point(const Vector2<T>& point) const
    {
        Vector2<T> c = point - mStart;
        Vector2<T> v = mEnd - mStart;
        T d = (T)v.get_length();
        v /= d;
        T t = v.dot_product(c);

        if(t < (T)0.0) return mStart;
        if(t > d) return mEnd;

        v *= t;
        return mStart + v;
    }

    //! Start point of the line.
    Vector2<T> mStart;
    //! End point of the line.
    Vector2<T> mEnd;
};

/** typedef **/
/** Line2f  **/
//! A floating point line.
typedef Line2<float> Line2f;

/** typedef **/
/** Line2i  **/
//! An integer line.
typedef Line2<int32_t> Line2i;

/** class **/
/** Line3 **/
//! A 3d line template class.
template <class T>
class Line3
{
public:
    /** default constructor **/
    Line3() :
        mStart(0,0,0),
        mEnd(1,1,1)
    {
        //
    }

    /** constructor **/
    /// \param    T xa
    ///         The x-coord of the first point on the line.
    /// \param    T ya
    ///         The y-coord of the first point on the line.
    /// \param    T za
    ///         The z-coord of the first point on the line.
    /// \param    T xb
    ///         The x-coord of the second point on the line.
    /// \param    T yb
    ///         The y-coord of the second point on the line.
    /// \param    T zb
    ///         The z-coord of the second point on the line.
    Line3(T xa,
          T ya,
          T za,
          T xb,
          T yb,
          T zb) :
        mStart(xa, ya, za),
        mEnd(xb, yb, zb)
    {
        //
    }

    /** constructor **/
    /// \param    const Vector3<T>& s
    ///         The first point on the line.
    /// \param    const Vector3<T>& e
    ///         The second point on the line.
    Line3(const Vector3<T>& s,
          const Vector3<T>& e) :
        mStart(s),
        mEnd(e)
    {
        //
    }

    /** constructor **/
    /// \param    const Line3<T>& other
    ///         The other line to copy.
    Line3(const Line3<T>& other) :
        mStart(other.mStart),
        mEnd(other.mEnd)
    {
        //
    }

    /** const operator **/
    /** +              **/
    //! Moves the line by the given vector.
    /// \return Line3<T>
    ///         The moved line.
    /// \param    const Vector3<T>& vec
    ///         The amount to move.
    Line3<T> operator+(const Vector3<T>& vec) const
    {
        return Line3<T>(mStart + vec, mEnd + vec);
    }

    /** operator **/
    /** +=       **/
    //! Moves this line with the given vector.
    /// \return Line3<T>&
    ///         A reference to this vector after the transformation.
    /// \param    const Vector3<T>& vec
    ///         The amount to move.
    Line3<T>& operator+=(const Vector3<T>& vec)
    {
        mStart += vec;
        mEnd += vec;
        return *this;
    }

    /** const operator **/
    /** -              **/
    //! Moves the line by the given vector.
    /// \return Line3<T>
    ///         The moved line.
    /// \param    const Vector3<T>& vec
    ///         The amount to move.
    Line3<T> operator-(const Vector3<T>& vec) const
    {
        return Line3<T>(mStart - vec, mEnd - vec);
    }

    /** operator **/
    /** -=       **/
    //! Moves this line with the given vector.
    /// \return Line3<T>&
    ///         A reference to this vector after the transformation.
    /// \param    const Vector3<T>& vec
    ///         The amount to move.
    Line3<T>& operator-=(const Vector3<T>& vec)
    {
        mStart -= vec;
        mEnd -= vec;
        return *this;
    }

    /** const operator **/
    /** ==             **/
    //! Checks if the two lines are the same.
    /// \return bool
    ///         True if the lines are the same, false otherwise.
    /// \param    const Line3<T>& other
    ///         The other line to check with.
    bool operator==(const Line3<T>& other) const
    {
        return (mStart==other.mStart && mEnd==other.mEnd) || (mEnd==other.mStart && mStart==other.mEnd);
    }
    /** const operator **/
    /** !=             **/
    //! Checks if the two lines are different.
    /// \return bool
    ///         True if the lines are different, false otherwise.
    /// \param    const Line3<T>& other
    ///         The other line to check with.
    bool operator!=(const Line3<T>& other) const
    {
        return !(mStart==other.mStart && mEnd==other.mEnd) || (mEnd==other.mStart && mStart==other.mEnd);
    }

    /** function **/
    /** set_line **/
    //! Set this line to new line going through the two points.
    /// \return void
    /// \param    T xa
    ///         The x-coord of the first point.
    /// \param    T ya
    ///         The y-coord of the first point.
    /// \param    T za
    ///         The z-coord of the first point.
    /// \param    T xb
    ///         The x-coord of the second point.
    /// \param    T yb
    ///         The y-coord of the second point.
    /// \param    T zb
    ///         The z-coord of the second point.
    void set_line(T xa,
                  T ya,
                  T za,
                  T xb,
                  T yb,
                  T zb)
    {
        mStart.set(xa, ya, za);
        mEnd.set(xb, yb, zb);
    }

    /** function **/
    /** set_line **/
    //! Set this line to new line going through the two points.
    /// \return void
    /// \param    const Vector3<T>& start
    ///         The first point.
    /// \param    const Vector3<T>& end
    ///         The second point.
    void set_line(const Vector3<T>& start,
                  const Vector3<T>& end)
    {
        mStart.set(start);
        mEnd.set(end);
    }

    /** function **/
    /** set_line **/
    //! Copies a line.
    /// \return void
    /// \param    const Line3<T>& line
    ///         The line to copy.
    void set_line(const Line3<T>& line)
    {
        mStart.set(line.mStart);
        mEnd.set(line.mEnd);
    }

    /** const function **/
    /** get_length     **/
    //! Returns the length of this line.
    /// \return double
    ///         The length of this line.
    /// \param  void
    T get_length(void) const
    {
        return mStart.get_distance_from(mEnd);
    }

    /** const function **/
    /** get_length_sq  **/
    //! Returns the squared length of this line.
    /// \return T
    ///         The squared length of this line.
    /// \param  void
    T get_length_sq(void) const
    {
        return mStart.get_distance_from_sq(mEnd);
    }

    /** const function **/
    /** get_center     **/
    //! Returns the center of this line.
    /// \return Vector3<T>
    ///         The center of this line.
    /// \param  void
    Vector3<T> get_center(void) const
    {
        return (mStart + mEnd) * (T)0.5;
    }

    /** const function **/
    /** get_vector     **/
    //! Returns the vector of this line.
    /// \return Vector3<T>
    ///         The vector of this line.
    /// \param  void
    Vector3<T> get_vector(void) const
    {
        return mEnd - mStart;
    }

    /** const function                 **/
    /** is_point_between_start_and_end **/
    //! Checks if the given point is somewhere between the start and the end of the line.
    /// \return bool
    ///         True if the point lies between start and end.
    /// \param    const Vector3<T>& point
    ///         The point to check.
    bool is_point_between_start_and_end(const Vector3<T>& point) const
    {
        return point.is_between_points(mStart, mEnd);
    }

    /** const function    **/
    /** get_closest_point **/
    //! Returns the closest point on this line to a point.
    /// \return Vector3<T>
    ///         The closest point on this line to a point.
    /// \param    const Vector3<T>& point
    ///         The point to check with.
    Vector3<T> get_closest_point(const Vector3<T>& point) const
    {
        Vector3<T> c = point - mStart;
        Vector3<T> v = mEnd - mStart;
        T d = (T)v.get_length();
        v /= d;
        T t = v.dot_product(c);

        if(t < (T)0.0)
            return mStart;
        if(t > d)
            return mEnd;

        v *= t;
        return mStart + v;
    }

    /** const function               **/
    /** get_intersection_with_sphere **/
    //! Returns if the line intersects with a sphere.
    /// \return bool
    ///         True if there is an intersection, false otherwise.
    /// \param    const Vector3<T>& sphere_origin
    ///         The origin of the sphere.
    /// \param    T sphere_radius
    ///         The radius of the sphere.
    /// \param    double& out_distance
    ///         If an intersection if found, the distance to the first intersection point is stored here.
    bool get_intersection_with_sphere(const Vector3<T>& sphere_origin,
                                      T sphere_radius,
                                      double& out_distance) const
    {
        const Vector3<T> q = sphere_origin - mStart;
        T c = q.get_length();
        T v = q.dot_product(get_vector().normalize());
        T d = sphere_radius * sphere_radius - (c*c - v*v);

        if(d < 0.0)
            return false;

        out_distance = v - Myth::Math::squareroot(d);
        return true;
    }

    //! Start point of line
    Vector3<T> mStart;
    //! End point of line
    Vector3<T> mEnd;
};

/** typedef **/
/** Line3f  **/
//! A floating point line.
typedef Line3<float> Line3f;

/** typedef **/
/** Line3i  **/
//! An integer line.
typedef Line3<int32_t> Line3i;

/** enumeration                  **/
/** LYNX_E_INTERSECTION_RELATION **/
//! A enumeration of all the intersection relationships.
enum LYNX_E_INTERSECTION_RELATION
{
    EIR_FRONT = 0,
    EIR_BACK,
    EIR_PLANAR,
    EIR_SPANNING,
    EIR_CLIPPED
};

/** class  **/
/** Plane3 **/
//! A 3d plane template class.
template <class T>
class Plane3
{
public:
    /** default constructor **/
    Plane3() :
        mNormal(0,1,0)
    {
        recalculate_d(Vector3<T>(0,0,0));
    }

    /** constructor **/
    /// \param    const Vector3<T>& mpoint
    ///         A point on the plane.
    /// \param    const Vector3<T>& normal
    ///         The normal vector of the plane.
    Plane3(const Vector3<T>& mpoint,
           const Vector3<T>& normal) :
        mNormal(normal)
    {
        recalculate_d(mpoint);
    }

    /** constructor **/
    /// \param    T px
    ///         The x-coord of a point on the plane.
    /// \param    T py
    ///         The y-coord of a point on the plane.
    /// \param    T pz
    ///         The z-coord of a point on the plane.
    /// \param    T nx
    ///         The x-coord of the normal of the plane.
    /// \param    T ny
    ///         The y-coord of the normal of the plane.
    /// \param    T nz
    ///         The z-coord of the normal of the plane.
    Plane3(T px,
           T py,
           T pz,
           T nx,
           T ny,
           T nz) :
        mNormal(nx, ny, nz)
    {
        recalculate_d(Vector3<T>(px, py, pz));
    }

    /** constructor **/
    /// \param    const Vector3<T>& point1
    ///         The first point on the plane.
    /// \param    const Vector3<T>& point2
    ///         The second point on the plane.
    /// \param    const Vector3<T>& point3
    ///         The third point on the plane.
    Plane3(const Vector3<T>& point1,
           const Vector3<T>& point2,
           const Vector3<T>& point3)
    {
        set_plane(point1, point2, point3);
    }

    /** constructor **/
    /// \param    const Vector3<T>& normal
    ///         The normal of the plane.
    /// \param    T d
    ///         The distance between the origin and this plane.
    Plane3(const Vector3<T>& normal,
           T d) :
        mNormal(normal),
        mD(d)
    {
        //
    }

    /** const operator **/
    /** ==             **/
    //! Checks if this plane equals the other one.
    /// \return bool
    ///         True if the planes are equal, false otherwise.
    /// \param    const Plane3<T>& other
    ///         The other plane you want to check with.
    bool operator==(const Plane3<T>& other) const
    {
        return (equals(mD, other.mD) && mNormal == other.mNormal);
    }

    /** const operator **/
    /** !=             **/
    //! Checks if this plane is different from the other one.
    /// \return bool
    ///         True if the planes are different, false otherwise.
    /// \param    const Plane3<T>& other
    ///         The other plane you want to check with.
    bool operator!=(const Plane3<T>& other) const
    {
        return !(*this == other);
    }

    /** function  **/
    /** set_plane **/
    //! Sets the plane using a point and a normal vector.
    /// \return void
    /// \param    const Vector3<T>& point
    ///         A point on the plane.
    /// \param    const Vector3<T>& nvector
    ///         The normal of the plane.
    void set_plane(const Vector3<T>& point,
                   const Vector3<T>& nvector)
    {
        mNormal = nvector;
        recalculate_d(point);
    }

    /** function  **/
    /** set_plane **/
    //! Sets the plane using a normal vector and a distance from the origin.
    /// \return void
    /// \param    const Vector3<T>& nvect
    ///         The normal of the plane.
    /// \param    T d
    ///         The distance between the origin and this plane.
    void set_plane(const Vector3<T>& nvect,
                   T d)
    {
        mNormal = nvect;
        mD = d;
    }

    /** function  **/
    /** set_plane **/
    //! Creates a plane from 3 memberpoints.
    /// \return void
    /// \param    const Vector3<T>& point1
    ///         The first point on the plane.
    /// \param    const Vector3<T>& point2
    ///         The second point on the plane.
    /// \param    const Vector3<T>& point3
    ///         The third point on the plane.
    void set_plane(const Vector3<T>& point1,
                   const Vector3<T>& point2,
                   const Vector3<T>& point3)
    {
        mNormal = (point2 - point1).cross_product(point3 - point1);
        mNormal.normalize();

        recalculate_d(point1);
    }

    /** const function             **/
    /** get_intersection_with_line **/
    //! Returns the intersection with a 3d line.
    /// \return bool
    ///         True if there was an intersection, false otherwise.
    /// \param    const Vector3<T>& line_point
    ///         Point of the line to intersect with.
    /// \param    const Vector3<T>& line_vect
    ///         Vector of the line to intersect with.
    /// \param    Vector3<T>& out_intersection
    ///         If there is an intersection, it will be stored here.
    bool get_intersection_with_line(const Vector3<T>& line_point,
                                    const Vector3<T>& line_vect,
                                    Vector3<T>& out_intersection) const
    {
        T t2 = mNormal.dot_product(line_vect);

        if(t2 == 0)
            return false;

        T t = -(mNormal.dot_product(line_point) + mD) / t2;
        out_intersection = line_point + (line_vect * t);
        return true;
    }

    /** const function                   **/
    /** get_known_intersection_with_line **/
    //! Returns the percentage of line between two points where an intersection with this plane happens.
    /// \return float
    ///         Where on a line between two points an intersection with this plane happened.
    /// \param    const Vector3<T>& line_point1
    ///         The first point on the line to intersect with.
    /// \param    const Vector3<T>& line_point2
    ///         The second point on the line to intersect with.
    float get_known_intersection_with_line(const Vector3<T>& line_point1,
                                           const Vector3<T>& line_point2) const
    {
        Vector3<T> vect = line_point2 - line_point1;
        T t2 = (float)mNormal.dot_product(vect);
        return (float)-((mNormal.dot_product(line_point1) + mD) / t2);
    }

    /** const function                     **/
    /** get_intersection_with_limited_line **/
    //! Returns the intersection with a limited 3d line.
    /// \return bool
    ///         True if there was an intersection, false otherwise.
    /// \param    const Vector3<T>& line_point1
    ///         The first point on the line.
    /// \param    const Vector3<T>& line_point2
    ///         The second point on the line.
    /// \param    Vector3<T>& out_intersection
    ///         If there was an intersection, it will be stored here.
    bool get_intersection_with_limited_line(const Vector3<T>& line_point1,
                                            const Vector3<T>& line_point2,
                                            Vector3<T>& out_intersection) const
    {
        return (get_intersection_with_line(line_point1, line_point2 - line_point1, out_intersection) &&
                out_intersection.is_between_points(line_point1, line_point2));
    }

    /** const function          **/
    /** classify_point_relation **/
    //! Classifies the relation of a point to this plane.
    /// \return LYNX_E_INTERSECTION_RELATION
    ///         EIR_FRONT if the point is in front of the plane, EIR_BACK if behind and EIR_PLANAR if on the plane.
    /// \param    const Vector3<T>& point
    ///         The point to classify its relation.
    LYNX_E_INTERSECTION_RELATION classify_point_relation(const Vector3<T>& point) const
    {
        const T d = mNormal.dot_product(point) + mD;

        if(d < -Myth::Math::ROUNDING_ERROR_32)
            return EIR_BACK;

        if(d > Myth::Math::ROUNDING_ERROR_32)
            return EIR_FRONT;

        return EIR_PLANAR;
    }

    /** function      **/
    /** recalculate_d **/
    //! Recalculates the distance from origin by applying a new member point to the plane.
    /// \return void
    /// \param    const Vector3<T>& point
    ///         The new member point on the plane.
    void recalculate_d(const Vector3<T>& point)
    {
        mD = -point.dot_product(mNormal);
    }

    /** const function   **/
    /** get_member_point **/
    //! Returns a member point on this plane.
    /// \return Vector3<T>
    ///         A member point on this plane.
    /// \param  void
    Vector3<T> get_member_point(void) const
    {
        return mNormal * -mD;
    }

    /** const function      **/
    /** exists_intersection **/
    //! Tests if there is an intersection with the other plane.
    /// \return bool
    ///         True if there is an intersection, false otherwise.
    /// \param    const Plane3<T>& other
    ///         The other plane to test with.
    bool exists_intersection(const Plane3<T>& other) const
    {
        Vector3<T> cross = other.mNormal.cross_product(mNormal);
        return cross.get_length() > Myth::Math::ROUNDING_ERROR_32;
    }

    /** const function              **/
    /** get_intersection_with_plane **/
    //! Intersects this plane with another.
    /// \return bool
    ///         True if there is an intersection, false otherwise.
    /// \param    const Plane3<T>& other
    ///         The other plane to intersect with.
    /// \param    Vector3<T>& out_line_point
    ///         Base point of the intersection line.
    /// \param    Vector3<T>& out_line_vect
    ///         Vector of intersection.
    bool get_intersection_with_plane(const Plane3<T>& other,
                                     Vector3<T>& out_line_point,
                                     Vector3<T>& out_line_vect) const
    {
        const T fn00 = mNormal.get_length();
        const T fn01 = mNormal.dot_product(other.mNormal);
        const T fn11 = other.mNormal.get_length();
        const double det = fn00*fn11 - fn01*fn01;

        if(fabs(det) < Myth::Math::ROUNDING_ERROR_64)
            return false;

        const double invdet = 1.0 / det;
        const double fc0 = (fn11*-mD + fn01*other.mD) * invdet;
        const double fc1 = (fn00*-other.mD + fn01*mD) * invdet;

        out_line_vect = mNormal.cross_product(other.mNormal);
        out_line_point = mNormal*(T)fc0 + other.mNormal*(T)fc1;
        return true;
    }

    /** const function               **/
    /** get_intersection_with_planes **/
    //! Returns the intersection point with two other planes if there is one.
    /// \return bool
    ///         True if there was an intersection, false otherwise.
    /// \param    const Plane3<T>& o1
    ///         First plane to intersect with.
    /// \param    const Plane3<T>& o2
    ///         Second plane to intersect with.
    /// \param    Vector3<T>& out_point
    ///         If there was an intersection, the point will be stored here.
    bool get_intersection_with_planes(const Plane3<T>& o1,
                                      const Plane3<T>& o2,
                                      Vector3<T>& out_point) const
    {
        Vector3<T> linePoint, lineVect;
        if(get_intersection_with_plane(o1, linePoint, lineVect))
            return o2.get_intersection_with_line(linePoint, lineVect, out_point);

        return false;
    }

    /** const function  **/
    /** is_front_facing **/
    //! Tests if the plane would be front or backfacing from any location.
    /// \return bool
    ///         True if the plane is front facing and false if it is back facing.
    /// \param    const Vector3<T>& look_direction
    ///         Look direction.
    bool is_front_facing(const Vector3<T>& look_direction) const
    {
        const float d = mNormal.dot_product(look_direction);
        return d <= 0.0f;
    }

    /** const function  **/
    /** get_distance_to **/
    //! Returns the distance to a point.
    /// \return T
    ///         The distance to a point.
    /// \param    const Vector3<T>& point
    ///         The point to get the distance from.
    T get_distance_to(const Vector3<T>& point) const
    {
        return point.dot_product(mNormal) + mD;
    }

    //! Normal vector of the plane.
    Vector3<T> mNormal;
    //! Distance from the origin.
    T mD;
};

/** typedef **/
/** Plane3f **/
//! A floating point 3d plane.
typedef Plane3<float> Plane3f;

/** typedef **/
/** Plane3i **/
//! An integer 3d plane.
typedef Plane3<int32_t> Plane3i;

/** class     **/
/** Rectangle **/
//! A rectangle template class.
template <class T>
class Rectangle
{
public:
    /** default constructor **/
    Rectangle() :
        UpperLeftCorner(0,0),
        LowerRightCorner(0,0)
    {
        //
    }

    /** constructor **/
    /// \param    T x
    ///         The x-coord of the upper left corner.
    /// \param    T y
    ///         The y-coord of the upper left corner.
    /// \param    T x2
    ///         The x-coord of the lower right corner.
    /// \param    T y2
    ///         The y-coord of the lower right corner.
    Rectangle(T x,
              T y,
              T x2,
              T y2) :
        UpperLeftCorner(x,y),
        LowerRightCorner(x2,y2)
    {
        //
    }

    /** constructor **/
    /// \param    const Vector2<T>& upper_left
    ///         The upper left corner.
    /// \param    const Vector2<T>& lower_right
    ///         The lower right corner.
    Rectangle(const Vector2<T>& upper_left,
              const Vector2<T>& lower_right) :
        UpperLeftCorner(upper_left),
        LowerRightCorner(lower_right)
    {
        //
    }

    /** const operator **/
    /** +              **/
    //! Returns a rectangle that was moved by the given vector.
    /// \return Rectangle<T>
    ///         A rectangle that was moved by the given vector.
    /// \param    const Vector2<T>& pos
    ///         The offset of the new rectangle.
    Rectangle<T> operator+(const Vector2<T>& pos) const
    {
        Rectangle<T> ret(*this);
        return ret+=pos;
    }

    /** operator **/
    /** +=       **/
    //! Moves this rectangle by the given vector.
    /// \return Rectangle<T>&
    ///         A reference to this rectangle after it was moved by the given vector.
    /// \param    const Vector2<T>& pos
    ///         The offset of this rectangle.
    Rectangle<T>& operator+=(const Vector2<T>& pos)
    {
        UpperLeftCorner += pos;
        LowerRightCorner += pos;
        return *this;
    }

    /** const operator **/
    /** -              **/
    //! Returns a rectangle that was moved by the given vector.
    /// \return Rectangle<T>
    ///         A rectangle that was moved by the given vector.
    /// \param    const Vector2<T>& pos
    ///         The offset of the new rectangle.
    Rectangle<T> operator-(const Vector2<T>& pos) const
    {
        Rectangle<T> ret(*this);
        return ret-=pos;
    }

    /** operator **/
    /** +=       **/
    //! Moves this rectangle by the given vector.
    /// \return Rectangle<T>&
    ///         A reference to this rectangle after it was moved by the given vector.
    /// \param    const Vector2<T>& pos
    ///         The offset of this rectangle.
    Rectangle<T>& operator-=(const Vector2<T>& pos)
    {
        UpperLeftCorner -= pos;
        LowerRightCorner -= pos;
        return *this;
    }

    /** const operator **/
    /** ==             **/
    //! Checks if the rectangles are the same.
    /// \return bool
    ///         True if the rectangles are the same, false otherwise.
    /// \param    const Rectangle<T>& other
    ///         The other rectangle to check with.
    bool operator==(const Rectangle<T>& other) const
    {
        return (UpperLeftCorner == other.UpperLeftCorner &&
                LowerRightCorner == other.LowerRightCorner);
    }

    /** const operator **/
    /** ==             **/
    //! Checks if the rectangles are different.
    /// \return bool
    ///         True if the rectangles are different, false otherwise.
    /// \param    const Rectangle<T>& other
    ///         The other rectangle to check with.
    bool operator!=(const Rectangle<T>& other) const
    {
        return (UpperLeftCorner != other.UpperLeftCorner ||
                LowerRightCorner != other.LowerRightCorner);
    }

    /** const operator **/
    /** <              **/
    //! Checks if this rectangle is smaller, in area, than the other one.
    /// \return bool
    ///         True if this rectangle is smaller, false otherwise.
    /// \param    const Rectangle<T>& other
    ///         The other rectangle to check with.
    bool operator<(const Rectangle<T>& other) const
    {
        return get_area() < other.get_area();
    }

    /** const function **/
    /** get_area       **/
    //! Returns the area of this rectangle.
    /// \return T
    ///         The area of this rectangle.
    /// \param  void
    T get_area(void) const
    {
        return get_width() * get_height();
    }

    /** const function  **/
    /** is_point_inside **/
    //! Returns if a point is within this rectangle.
    /// \return bool
    ///         True if the point is within this rectangle, false otherwise.
    /// \param    const Vector2<T>& pos
    ///         The point to check.
    bool is_point_inside(const Vector2<T>& pos) const
    {
        return (UpperLeftCorner.X <= pos.X &&
                UpperLeftCorner.Y <= pos.Y &&
                LowerRightCorner.X >= pos.X &&
                LowerRightCorner.Y >= pos.Y);
    }

    /** const function   **/
    /** is_rect_collided **/
    //! Checks to see if this rectangle collides with another rectangle.
    /// \return bool
    ///         True if the rectangles collide, false otherwise.
    /// \param    const Rectangle<T>& other
    ///         The other rectangle to check with.
    bool is_rect_collided(const Rectangle<T>& other) const
    {
        return (LowerRightCorner.Y > other.UpperLeftCorner.Y &&
                UpperLeftCorner.Y < other.LowerRightCorner.Y &&
                LowerRightCorner.X > other.UpperLeftCorner.X &&
                UpperLeftCorner.X < other.LowerRightCorner.X);
    }

    /** function     **/
    /** clip_against **/
    //! Clips this rectangle with another one.
    /// \return void
    /// \param    const Rectangle<T>& other
    ///         The other rectangle to clip with.
    void clip_against(const Rectangle<T>& other)
    {
        if(other.LowerRightCorner.X < LowerRightCorner.X)
            LowerRightCorner.X = other.LowerRightCorner.X;
        if(other.LowerRightCorner.Y < LowerRightCorner.Y)
            LowerRightCorner.Y = other.LowerRightCorner.Y;

        if(other.UpperLeftCorner.X > UpperLeftCorner.X)
            UpperLeftCorner.X = other.UpperLeftCorner.X;
        if(other.UpperLeftCorner.Y > UpperLeftCorner.Y)
            UpperLeftCorner.Y = other.UpperLeftCorner.Y;

        // correct possible invalid rect resulting from clipping
        if(UpperLeftCorner.Y > LowerRightCorner.Y)
            UpperLeftCorner.Y = LowerRightCorner.Y;
        if(UpperLeftCorner.X > LowerRightCorner.X)
            UpperLeftCorner.X = LowerRightCorner.X;
    }

    /** function     **/
    /** constrain_to **/
    //! Constrains this rectangle to fit inside another one.
    /// \return bool
    ///         True on succes, false if not possible.
    /// \param    const Rectangle<T>& other
    ///         The other rectangle to fit in.
    bool constrain_to(const Rectangle<T>& other)
    {
        if(other.get_width() < get_width() || other.get_height() < get_height())
            return false;

        T diff = other.LowerRightCorner.X - LowerRightCorner.X;
        if(diff < 0)
        {
            LowerRightCorner.X += diff;
            UpperLeftCorner.X  += diff;
        }

        diff = other.LowerRightCorner.Y - LowerRightCorner.Y;
        if(diff < 0)
        {
            LowerRightCorner.Y += diff;
            UpperLeftCorner.Y  += diff;
        }

        diff = UpperLeftCorner.X - other.UpperLeftCorner.X;
        if(diff < 0)
        {
            UpperLeftCorner.X  -= diff;
            LowerRightCorner.X -= diff;
        }

        diff = UpperLeftCorner.Y - other.UpperLeftCorner.Y;
        if(diff < 0)
        {
            UpperLeftCorner.Y  -= diff;
            LowerRightCorner.Y -= diff;
        }

        return true;
    }

    /** const function **/
    /** get_width      **/
    //! Returns the width of this rectangle.
    /// \return T
    ///         The width of this rectangle.
    /// \param  void
    T get_width(void) const
    {
        return LowerRightCorner.X - UpperLeftCorner.X;
    }

    /** const function **/
    /** get_height     **/
    //! Returns the height of this rectangle.
    /// \return T
    ///         The height of this rectangle.
    /// \param  void
    T get_height(void) const
    {
        return LowerRightCorner.Y - UpperLeftCorner.Y;
    }

    /** function **/
    /** repair   **/
    //! Repairs the rectangle to make sure it is valid.
    /// \return void
    /// \param  void
    void repair(void)
    {
        if(LowerRightCorner.X < UpperLeftCorner.X)
        {
            T t = LowerRightCorner.X;
            LowerRightCorner.X = UpperLeftCorner.X;
            UpperLeftCorner.X = t;
        }

        if(LowerRightCorner.Y < UpperLeftCorner.Y)
        {
            T t = LowerRightCorner.Y;
            LowerRightCorner.Y = UpperLeftCorner.Y;
            UpperLeftCorner.Y = t;
        }
    }

    /** const function **/
    /** is_valid       **/
    //! Returns if the rectangle is valid.
    /// \return bool
    ///         True if the rectangle is valid, false otherwise.
    /// \param  void
    bool is_valid(void) const
    {
        return((LowerRightCorner.X >= UpperLeftCorner.X) &&
               (LowerRightCorner.Y >= UpperLeftCorner.Y));
    }

    /** const function **/
    /** get_center     **/
    //! Returns the center of this rectangle.
    /// \return Vector2<T>
    ///         The center of this rectangle.
    /// \param  void
    Vector2<T> get_center(void) const
    {
        return Vector2<T>(
                   (UpperLeftCorner.X + LowerRightCorner.X) / 2,
                   (UpperLeftCorner.Y + LowerRightCorner.Y) / 2);
    }

    /** const function **/
    /** get_size       **/
    //! Returns the size of this rectangle.
    /// \return Vector2<T>
    ///         The size of this rectangle.
    /// \param  void
    Vector2<T> get_size(void) const
    {
        return Vector2<T>(get_width(), get_height());
    }

    /** function           **/
    /** add_internal_point **/
    //! Adds a point to the rectangle.
    /// \return void
    /// \param    const Vector2<T>& p
    ///         The point to add.
    void add_internal_point(const Vector2<T>& p)
    {
        add_internal_point(p.X, p.Y);
    }

    /** function           **/
    /** add_internal_point **/
    //! Adds a point to the rectangle.
    /// \return void
    /// \param    T x
    ///         The x-coord of the point to add.
    /// \param    T y
    ///         The y-coord of the point to add.
    void add_internal_point(T x,
                            T y)
    {
        if(x>LowerRightCorner.X)
            LowerRightCorner.X = x;
        if(y>LowerRightCorner.Y)
            LowerRightCorner.Y = y;

        if(x<UpperLeftCorner.X)
            UpperLeftCorner.X = x;
        if(y<UpperLeftCorner.Y)
            UpperLeftCorner.Y = y;
    }

    //! Upper left corner
    Vector2<T> UpperLeftCorner;
    //! Lower right corner
    Vector2<T> LowerRightCorner;
};

/** typedef    **/
/** Rectanglef **/
//! A floating point rectangle.
typedef Rectangle<float> Rectanglef;
/** typedef    **/
/** Rectanglei **/
//! An integer rectangle.
typedef Rectangle<int32_t> Rectanglei;
/** typedef    **/
/** Rectangleu **/
//! An unsigned integer rectangle.
typedef Rectangle<uint32_t> Rectangleu;

//! Axis aligned bounding box in 3d dimensional space.
/** Has some useful methods used with occlusion culling or clipping.
*/
template <class T>
class BoundingBox3
{
public:

    //! Default Constructor.
    BoundingBox3(): MinEdge(-1,-1,-1), MaxEdge(1,1,1) {}
    //! Constructor with min edge and max edge.
    BoundingBox3(const Vector3<T>& min, const Vector3<T>& max): MinEdge(min), MaxEdge(max) {}
    //! Constructor with only one point.
    BoundingBox3(const Vector3<T>& init): MinEdge(init), MaxEdge(init) {}
    //! Constructor with min edge and max edge as single values, not vectors.
    BoundingBox3(T minx, T miny, T minz, T maxx, T maxy, T maxz): MinEdge(minx, miny, minz), MaxEdge(maxx, maxy, maxz) {}

    // operators
    //! Equality operator
    /** \param other box to compare with.
    \return True if both boxes are equal, else false. */
    inline bool operator==(const BoundingBox3<T>& other) const
    {
        return (MinEdge == other.MinEdge && other.MaxEdge == MaxEdge);
    }
    //! Inequality operator
    /** \param other box to compare with.
    \return True if both boxes are different, else false. */
    inline bool operator!=(const BoundingBox3<T>& other) const
    {
        return !(MinEdge == other.MinEdge && other.MaxEdge == MaxEdge);
    }

    // functions

    //! Resets the bounding box to a one-point box.
    /** \param x X coord of the point.
    \param y Y coord of the point.
    \param z Z coord of the point. */
    void reset(T x, T y, T z)
    {
        MaxEdge.set(x,y,z);
        MinEdge = MaxEdge;
    }

    //! Resets the bounding box.
    /** \param initValue New box to set this one to. */
    void reset(const BoundingBox3<T>& initValue)
    {
        *this = initValue;
    }

    //! Resets the bounding box to a one-point box.
    /** \param initValue New point. */
    void reset(const Vector3<T>& initValue)
    {
        MaxEdge = initValue;
        MinEdge = initValue;
    }

    //! Adds a point to the bounding box
    /** The box grows bigger, if point was outside of the box.
    \param p: Point to add into the box. */
    void addInternalPoint(const Vector3<T>& p)
    {
        addInternalPoint(p.X, p.Y, p.Z);
    }

    //! Adds another bounding box
    /** The box grows bigger, if the new box was outside of the box.
    \param b: Other bounding box to add into this box. */
    void addInternalBox(const BoundingBox3<T>& b)
    {
        addInternalPoint(b.MaxEdge);
        addInternalPoint(b.MinEdge);
    }

    //! Adds a point to the bounding box
    /** The box grows bigger, if point is outside of the box.
    \param x X coordinate of the point to add to this box.
    \param y Y coordinate of the point to add to this box.
    \param z Z coordinate of the point to add to this box. */
    void addInternalPoint(T x, T y, T z)
    {
        if(x>MaxEdge.X) MaxEdge.X = x;
        if(y>MaxEdge.Y) MaxEdge.Y = y;
        if(z>MaxEdge.Z) MaxEdge.Z = z;

        if(x<MinEdge.X) MinEdge.X = x;
        if(y<MinEdge.Y) MinEdge.Y = y;
        if(z<MinEdge.Z) MinEdge.Z = z;
    }

    //! Get center of the bounding box
    /** \return Center of the bounding box. */
    Vector3<T> getCenter() const
    {
        return (MinEdge + MaxEdge) / 2;
    }

    //! Get extent of the box (maximal distance of two points in the box)
    /** \return Extent of the bounding box. */
    Vector3<T> getExtent() const
    {
        return MaxEdge - MinEdge;
    }

    //! Check if the box is empty.
    /** This means that there is no space between the min and max edge.
    \return True if box is empty, else false. */
    bool isEmpty() const
    {
        return MinEdge.equals(MaxEdge);
    }

    //! Get the volume enclosed by the box in cubed units
    T getVolume() const
    {
        const Vector3<T> e = getExtent();
        return e.X * e.Y * e.Z;
    }

    //! Get the surface area of the box in squared units
    T getArea() const
    {
        const Vector3<T> e = getExtent();
        return 2*(e.X*e.Y + e.X*e.Z + e.Y*e.Z);
    }

    //! Stores all 8 edges of the box into an array
    /** \param edges: Pointer to array of 8 edges. */
    void getEdges(Vector3<T> *edges) const
    {
        const Vector3<T> middle = getCenter();
        const Vector3<T> diag = middle - MaxEdge;

        /*
        Edges are stored in this way:
        Hey, am I an ascii artist, or what? :) niko.
               /3--------/7
              / |       / |
             /  |      /  |
            1---------5   |
            |  /2- - -|- -6
            | /       |  /
            |/        | /
            0---------4/
        */

        edges[0].set(middle.X + diag.X, middle.Y + diag.Y, middle.Z + diag.Z);
        edges[1].set(middle.X + diag.X, middle.Y - diag.Y, middle.Z + diag.Z);
        edges[2].set(middle.X + diag.X, middle.Y + diag.Y, middle.Z - diag.Z);
        edges[3].set(middle.X + diag.X, middle.Y - diag.Y, middle.Z - diag.Z);
        edges[4].set(middle.X - diag.X, middle.Y + diag.Y, middle.Z + diag.Z);
        edges[5].set(middle.X - diag.X, middle.Y - diag.Y, middle.Z + diag.Z);
        edges[6].set(middle.X - diag.X, middle.Y + diag.Y, middle.Z - diag.Z);
        edges[7].set(middle.X - diag.X, middle.Y - diag.Y, middle.Z - diag.Z);
    }

    //! Repairs the box.
    /** Necessary if for example MinEdge and MaxEdge are swapped. */
    void repair()
    {
        T t;

        if(MinEdge.X > MaxEdge.X)
        {
            t=MinEdge.X;
            MinEdge.X = MaxEdge.X;
            MaxEdge.X=t;
        }
        if(MinEdge.Y > MaxEdge.Y)
        {
            t=MinEdge.Y;
            MinEdge.Y = MaxEdge.Y;
            MaxEdge.Y=t;
        }
        if(MinEdge.Z > MaxEdge.Z)
        {
            t=MinEdge.Z;
            MinEdge.Z = MaxEdge.Z;
            MaxEdge.Z=t;
        }
    }

    //! Calculates a new interpolated bounding box.
    /** d=0 returns other, d=1 returns this, all other values blend between
    the two boxes.
    \param other Other box to interpolate between
    \param d Value between 0.0f and 1.0f.
    \return Interpolated box. */
    BoundingBox3<T> getInterpolated(const BoundingBox3<T>& other, float d) const
    {
        float inv = 1.0f - d;
        return BoundingBox3<T>((other.MinEdge*inv) + (MinEdge*d),
                               (other.MaxEdge*inv) + (MaxEdge*d));
    }

    //! Determines if a point is within this box.
    /** Border is included (IS part of the box)!
    \param p: Point to check.
    \return True if the point is within the box and false if not */
    bool isPointInside(const Vector3<T>& p) const
    {
        return (p.X >= MinEdge.X && p.X <= MaxEdge.X &&
                p.Y >= MinEdge.Y && p.Y <= MaxEdge.Y &&
                p.Z >= MinEdge.Z && p.Z <= MaxEdge.Z);
    }

    //! Determines if a point is within this box and not its borders.
    /** Border is excluded (NOT part of the box)!
    \param p: Point to check.
    \return True if the point is within the box and false if not. */
    bool isPointTotalInside(const Vector3<T>& p) const
    {
        return (p.X > MinEdge.X && p.X < MaxEdge.X &&
                p.Y > MinEdge.Y && p.Y < MaxEdge.Y &&
                p.Z > MinEdge.Z && p.Z < MaxEdge.Z);
    }

    //! Check if this box is completely inside the 'other' box.
    /** \param other: Other box to check against.
    \return True if this box is completly inside the other box,
    otherwise false. */
    bool isFullInside(const BoundingBox3<T>& other) const
    {
        return (MinEdge.X >= other.MinEdge.X && MinEdge.Y >= other.MinEdge.Y && MinEdge.Z >= other.MinEdge.Z &&
                MaxEdge.X <= other.MaxEdge.X && MaxEdge.Y <= other.MaxEdge.Y && MaxEdge.Z <= other.MaxEdge.Z);
    }

    //! Determines if the axis-aligned box intersects with another axis-aligned box.
    /** \param other: Other box to check a intersection with.
    \return True if there is an intersection with the other box,
    otherwise false. */
    bool intersectsWithBox(const BoundingBox3<T>& other) const
    {
        return (MinEdge.X <= other.MaxEdge.X && MinEdge.Y <= other.MaxEdge.Y && MinEdge.Z <= other.MaxEdge.Z &&
                MaxEdge.X >= other.MinEdge.X && MaxEdge.Y >= other.MinEdge.Y && MaxEdge.Z >= other.MinEdge.Z);
    }

    //! Tests if the box intersects with a line
    /** \param line: Line to test intersection with.
    \return True if there is an intersection , else false. */
    bool intersectsWithLine(const Line3<T>& line) const
    {
        return intersectsWithLine(line.get_center(), line.get_vector().normalize(),
                                  (T)(line.get_length() * 0.5));
    }

    //! Tests if the box intersects with a line
    /** \param linemiddle Center of the line.
    \param linevect Vector of the line.
    \param halflength Half length of the line.
    \return True if there is an intersection, else false. */
    bool intersectsWithLine(const Vector3<T>& linemiddle,
                            const Vector3<T>& linevect, T halflength) const
    {
        const Vector3<T> e = getExtent() * (T)0.5;
        const Vector3<T> t = getCenter() - linemiddle;

        if((fabs(t.X) > e.X + halflength * fabs(linevect.X)) ||
                (fabs(t.Y) > e.Y + halflength * fabs(linevect.Y)) ||
                (fabs(t.Z) > e.Z + halflength * fabs(linevect.Z)))
            return false;

        T r = e.Y * (T)fabs(linevect.Z) + e.Z * (T)fabs(linevect.Y);
        if(fabs(t.Y*linevect.Z - t.Z*linevect.Y) > r)
            return false;

        r = e.X * (T)fabs(linevect.Z) + e.Z * (T)fabs(linevect.X);
        if(fabs(t.Z*linevect.X - t.X*linevect.Z) > r)
            return false;

        r = e.X * (T)fabs(linevect.Y) + e.Y * (T)fabs(linevect.X);
        if(fabs(t.X*linevect.Y - t.Y*linevect.X) > r)
            return false;

        return true;
    }

    //! Classifies a relation with a plane.
    /** \param plane Plane to classify relation to.
    \return Returns ISREL3D_FRONT if the box is in front of the plane,
    ISREL3D_BACK if the box is behind the plane, and
    ISREL3D_CLIPPED if it is on both sides of the plane. */
    LYNX_E_INTERSECTION_RELATION classifyPlaneRelation(const Plane3<T>& plane) const
    {
        Vector3<T> nearPoint(MaxEdge);
        Vector3<T> farPoint(MinEdge);

        if(plane.Normal.X > (T)0)
        {
            nearPoint.X = MinEdge.X;
            farPoint.X = MaxEdge.X;
        }

        if(plane.Normal.Y > (T)0)
        {
            nearPoint.Y = MinEdge.Y;
            farPoint.Y = MaxEdge.Y;
        }

        if(plane.Normal.Z > (T)0)
        {
            nearPoint.Z = MinEdge.Z;
            farPoint.Z = MaxEdge.Z;
        }

        if(plane.Normal.dotProduct(nearPoint) + plane.D > (T)0)
            return EIR_FRONT;

        if(plane.Normal.dotProduct(farPoint) + plane.D > (T)0)
            return EIR_CLIPPED;

        return EIR_BACK;
    }

    //! The near edge
    Vector3<T> MinEdge;

    //! The far edge
    Vector3<T> MaxEdge;
};

//! Typedef for a f32 3d bounding box.
typedef BoundingBox3<float> BoundingBox3f;
//! Typedef for an integer 3d bounding box.
typedef BoundingBox3<int32_t> BoundingBox3i;

/** class     **/
/** Triangle3 **/
//! A 3d triangle template class.
template <class T>
class Triangle3
{
public:
    /** default constructor **/
    Triangle3()
    {
        //
    }

    /** constructor **/
    /// \param    const Vector3<T>& v1
    ///         The first point of this triangle.
    /// \param    const Vector3<T>& v2
    ///         The second point of this triangle.
    /// \param    const Vector3<T>& v3
    ///         The third point of this triangle.
    Triangle3(const Vector3<T>& v1,
              const Vector3<T>& v2,
              const Vector3<T>& v3) :
        mPointA(v1),
        mPointB(v2),
        mPointC(v3)
    {
        //
    }

    //! Equality operator
    /** const operator **/
    /** ==             **/
    //! Checks if this triangle equals another.
    /// \return bool
    ///         True if they're equal, false otherwise.
    /// \param    const Triangle3<T>& other
    ///         The other triangle to test with.
    bool operator==(const Triangle3<T>& other) const
    {
        return other.mPointA==mPointA && other.mPointB==mPointB && other.mPointC==mPointC;
    }

    /** const operator **/
    /** !=             **/
    //! Checks if this triangle is different from the other one.
    /// \return bool
    ///         True if the triangles are different, false otherwise.
    /// \param    const Triangle3<T>& other
    ///         The other plane you want to check with.
    bool operator!=(const Triangle3<T>& other) const
    {
        return !(*this==other);
    }

    //! Determines if the triangle is totally inside a bounding box.
    /** \param box Box to check.
    \return True if triangle is within the box, otherwise false. */
    bool is_total_inside_box(const BoundingBox3<T>& box) const
    {
        return (box.isPointInside(mPointA) &&
                box.isPointInside(mPointB) &&
                box.isPointInside(mPointC));
    }

    //! Determines if the triangle is totally outside a bounding box.
    /** \param box Box to check.
    \return True if triangle is outside the box, otherwise false. */
    bool is_total_outside_box(const BoundingBox3<T>& box) const
    {
        return ((mPointA.X > box.MaxEdge.X && mPointB.X > box.MaxEdge.X && mPointC.X > box.MaxEdge.X) ||

                (mPointA.Y > box.MaxEdge.Y && mPointB.Y > box.MaxEdge.Y && mPointC.Y > box.MaxEdge.Y) ||
                (mPointA.Z > box.MaxEdge.Z && mPointB.Z > box.MaxEdge.Z && mPointC.Z > box.MaxEdge.Z) ||
                (mPointA.X < box.MinEdge.X && mPointB.X < box.MinEdge.X && mPointC.X < box.MinEdge.X) ||
                (mPointA.Y < box.MinEdge.Y && mPointB.Y < box.MinEdge.Y && mPointC.Y < box.MinEdge.Y) ||
                (mPointA.Z < box.MinEdge.Z && mPointB.Z < box.MinEdge.Z && mPointC.Z < box.MinEdge.Z));
    }

    //! Get the closest point on a triangle to a point on the same plane.
    /** \param p Point which must be on the same plane as the triangle.
    \return The closest point of the triangle */
    Vector3<T> closest_point_on_triangle(const Vector3<T>& p) const
    {
        const Vector3<T> rab = Line3<T>(mPointA, mPointB).get_closest_point(p);
        const Vector3<T> rbc = Line3<T>(mPointB, mPointC).get_closest_point(p);
        const Vector3<T> rca = Line3<T>(mPointC, mPointA).get_closest_point(p);

        const T d1 = rab.get_distance_from(p);
        const T d2 = rbc.get_distance_from(p);
        const T d3 = rca.get_distance_from(p);

        if(d1 < d2)
            return d1 < d3 ? rab : rca;

        return d2 < d3 ? rbc : rca;
    }

    //! Check if a point is inside the triangle
    /** \param p Point to test. Assumes that this point is already
    on the plane of the triangle.
    \return True if the point is inside the triangle, otherwise false. */
    bool is_point_inside(const Vector3<T>& p) const
    {
        return (is_on_same_side(p, mPointA, mPointB, mPointC) &&
                is_on_same_side(p, mPointB, mPointA, mPointC) &&
                is_on_same_side(p, mPointC, mPointA, mPointB));
    }

    //! Check if a point is inside the triangle.
    /** This method is an implementation of the example used in a
    paper by Kasper Fauerby original written by Keidy from
    Mr-Gamemaker.
    \param p Point to test. Assumes that this point is already
    on the plane of the triangle.
    \return True if point is inside the triangle, otherwise false. */
    bool is_point_inside_fast(const Vector3<T>& p) const
    {
        const Vector3<T> f = mPointB - mPointA;
        const Vector3<T> g = mPointC - mPointA;

        const float a = f.dot_product(f);
        const float b = f.dot_product(g);
        const float c = g.dot_product(g);

        const Vector3<T> vp = p - mPointA;
        const float d = vp.dot_product(f);
        const float e = vp.dot_product(g);

        float x = (d*c)-(e*b);
        float y = (e*a)-(d*b);
        const float ac_bb = (a*c)-(b*b);
        float z = x+y-ac_bb;

        return ((((uint32_t&)(z)) & ~(((uint32_t&)(x))|((uint32_t&)(y)))) & 0x80000000)!=0;
    }

    //! Get an intersection with a 3d line.
    /** \param line Line to intersect with.
    \param outIntersection Place to store the intersection point, if there is one.
    \return True if there was an intersection, false if not. */
    bool get_intersection_with_limited_line(const Line3<T>& line,
                                            Vector3<T>& out_intersection) const
    {
        return get_intersection_with_line(line.start, line.get_vector(), out_intersection) &&
               out_intersection.is_between_points(line.start, line.end);
    }


    //! Get an intersection with a 3d line.
    /** Please note that also points are returned as intersection which
    are on the line, but not between the start and end point of the line.
    If you want the returned point be between start and end
    use getIntersectionWithLimitedLine().
    \param linePoint Point of the line to intersect with.
    \param lineVect Vector of the line to intersect with.
    \param outIntersection Place to store the intersection point, if there is one.
    \return True if there was an intersection, false if there was not. */
    bool get_intersection_with_line(const Vector3<T>& line_point,
                                    const Vector3<T>& line_vect,
                                    Vector3<T>& out_intersection) const
    {
        if(get_intersection_of_plane_with_line(line_point, line_vect, out_intersection))
            return is_point_inside(out_intersection);

        return false;
    }


    //! Calculates the intersection between a 3d line and the plane the triangle is on.
    /** \param lineVect Vector of the line to intersect with.
    \param linePoint Point of the line to intersect with.
    \param outIntersection Place to store the intersection point, if there is one.
    \return True if there was an intersection, else false. */
    bool get_intersection_of_plane_with_line(const Vector3<T>& line_point,
            const Vector3<T>& line_vect,
            Vector3<T>& out_intersection) const
    {
        const Vector3<T> normal = get_normal().normalize();
        T t2;

        if(Myth::Math::is_zero(t2 = normal.dot_product(line_vect)))
            return false;

        T d = mPointA.dot_product(normal);
        T t = -(normal.dot_product(line_point) - d) / t2;
        out_intersection = line_point + (line_vect * t);
        return true;
    }


    //! Get the normal of the triangle.
    /** Please note: The normal is not always normalized. */
    Vector3<T> get_normal(void) const
    {
        return (mPointB - mPointA).cross_product(mPointC - mPointA);
    }

    //! Test if the triangle would be front or backfacing from any point.
    /** Thus, this method assumes a camera position from which the
    triangle is definitely visible when looking at the given direction.
    Do not use this method with points as it will give wrong results!
    \param lookDirection Look direction.
    \return True if the plane is front facing and false if it is backfacing. */
    bool is_front_facing(const Vector3<T>& look_direction) const
    {
        const Vector3<T> n = get_normal().normalize();
        const float d = (float)n.dot_product(look_direction);
        return d <= 0;
    }

    //! Get the plane of this triangle.
    Plane3<T> get_plane(void) const
    {
        return Plane3<T>(mPointA, mPointB, mPointC);
    }

    //! Get the area of the triangle
    T get_area(void) const
    {
        return (mPointB - mPointA).cross_product(mPointC - mPointA).get_length() * 0.5;

    }

    //! sets the triangle's points
    void set(const Vector3<T>& a,
             const Vector3<T>& b,
             const Vector3<T>& c)
    {
        mPointA = a;
        mPointB = b;
        mPointC = c;
    }

    //! the three points of the triangle
    Vector3<T> mPointA;
    Vector3<T> mPointB;
    Vector3<T> mPointC;

private:
    bool is_on_same_side(const Vector3<T>& p1,
                         const Vector3<T>& p2,
                         const Vector3<T>& a,
                         const Vector3<T>& b) const
    {
        Vector3<T> bminusa = b - a;
        Vector3<T> cp1 = bminusa.cross_product(p1 - a);
        Vector3<T> cp2 = bminusa.cross_product(p2 - a);
        return (cp1.dot_product(cp2) >= 0.0f);
    }
};

/** typedef    **/
/** Triangle3f **/
//! A floating point 3d triangle.
typedef Triangle3<float> Triangle3f;

/** typedef    **/
/** Triangle3i **/
//! An integer 3d triangle.
typedef Triangle3<int32_t> Triangle3i;

//! 4x4 matrix. Mostly used as transformation matrix for 3d calculations.
/** The matrix is a D3D style matrix, row major with translations in the 4th row. */
template <class T>
class CMatrix4
{
public:

    //! Constructor Flags
    enum eConstructor
    {
        EM4CONST_NOTHING = 0,
        EM4CONST_COPY,
        EM4CONST_IDENTITY,
        EM4CONST_TRANSPOSED,
        EM4CONST_INVERSE,
        EM4CONST_INVERSE_TRANSPOSED
    };

    //! Default constructor
    /** \param constructor Choose the initialization style */
    CMatrix4(eConstructor constructor = EM4CONST_IDENTITY);
    //! Copy constructor
    /** \param other Other matrix to copy from
    \param constructor Choose the initialization style */
    CMatrix4(const CMatrix4<T>& other,eConstructor constructor = EM4CONST_COPY);

    //! Simple operator for diRectanglely accessing every element of the matrix.
    T& operator()(const int32_t row, const int32_t col)
    {
        return M[ row * 4 + col ];
    }

    //! Simple operator for diRectanglely accessing every element of the matrix.
    const T& operator()(const int32_t row, const int32_t col) const
    {
        return M[row * 4 + col];
    }

    //! Simple operator for linearly accessing every element of the matrix.
    T& operator[](uint32_t index)
    {
        return M[index];
    }

    //! Simple operator for linearly accessing every element of the matrix.
    const T& operator[](uint32_t index) const
    {
        return M[index];
    }

    //! Sets this matrix equal to the other matrix.
    inline CMatrix4<T>& operator=(const CMatrix4<T> &other);

    //! Sets all elements of this matrix to the value.
    inline CMatrix4<T>& operator=(const T& scalar);

    //! Returns pointer to internal array
    const T* pointer() const
    {
        return M;
    }
    T* pointer()
    {
        return M;
    }

    //! Returns true if other matrix is equal to this matrix.
    bool operator==(const CMatrix4<T> &other) const;

    //! Returns true if other matrix is not equal to this matrix.
    bool operator!=(const CMatrix4<T> &other) const;

    //! Add another matrix.
    CMatrix4<T> operator+(const CMatrix4<T>& other) const;

    //! Add another matrix.
    CMatrix4<T>& operator+=(const CMatrix4<T>& other);

    //! Subtract another matrix.
    CMatrix4<T> operator-(const CMatrix4<T>& other) const;

    //! Subtract another matrix.
    CMatrix4<T>& operator-=(const CMatrix4<T>& other);

    //! set this matrix to the product of two matrices
    inline CMatrix4<T>& setbyproduct(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b);

    //! Set this matrix to the product of two matrices
    /** no optimization used,
    use it if you know you never have a identity matrix */
    CMatrix4<T>& setbyproduct_nocheck(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b);

    //! Multiply by another matrix.
    CMatrix4<T> operator*(const CMatrix4<T>& other) const;

    //! Multiply by another matrix.
    CMatrix4<T>& operator*=(const CMatrix4<T>& other);

    //! Multiply by scalar.
    CMatrix4<T> operator*(const T& scalar) const;

    //! Multiply by scalar.
    CMatrix4<T>& operator*=(const T& scalar);

    //! Set matrix to identity.
    inline CMatrix4<T>& makeIdentity();

    //! Returns true if the matrix is the identity matrix
    inline bool isIdentity() const;

    //! Returns true if the matrix is orthogonal
    inline bool isOrthogonal() const;

    //! Returns true if the matrix is the identity matrix
    bool isIdentity_integer_base() const;

    //! Set the translation of the current matrix. Will erase any previous values.
    CMatrix4<T>& setTranslation(const Vector3<T>& translation);

    //! Gets the current translation
    Vector3<T> getTranslation() const;

    //! Set the inverse translation of the current matrix. Will erase any previous values.
    CMatrix4<T>& setInverseTranslation(const Vector3<T>& translation);

    //! Make a rotation matrix from Euler angles. The 4th row and column are unmodified.
    inline CMatrix4<T>& setRotationRadians(const Vector3<T>& rotation);

    //! Make a rotation matrix from Euler angles. The 4th row and column are unmodified.
    CMatrix4<T>& setRotationDegrees(const Vector3<T>& rotation);

    //! Returns the rotation, as set by setRotation().
    /** This code was orginally written by by Chev. */
    Vector3<T> getRotationDegrees() const;

    //! Make an inverted rotation matrix from Euler angles.
    /** The 4th row and column are unmodified. */
    inline CMatrix4<T>& setInverseRotationRadians(const Vector3<T>& rotation);

    //! Make an inverted rotation matrix from Euler angles.
    /** The 4th row and column are unmodified. */
    CMatrix4<T>& setInverseRotationDegrees(const Vector3<T>& rotation);

    //! Set Scale
    CMatrix4<T>& setScale(const Vector3<T>& scale);

    //! Set Scale
    CMatrix4<T>& setScale(const T scale)
    {
        return setScale(Vector3<T>(scale,scale,scale));
    }

    //! Get Scale
    Vector3<T> getScale() const;

    //! Translate a vector by the inverse of the translation part of this matrix.
    void inverseTranslateVect(Vector3f& vect) const;

    //! Rotate a vector by the inverse of the rotation part of this matrix.
    void inverseRotateVect(Vector3f& vect) const;

    //! Rotate a vector by the rotation part of this matrix.
    void rotateVect(Vector3f& vect) const;

    //! An alternate transform vector method, writing into a second vector
    void rotateVect(Vector3f& out, const Vector3f& in) const;

    //! An alternate transform vector method, writing into an array of 3 floats
    void rotateVect(T* out,const Vector3f& in) const;

    //! Transforms the vector by this matrix
    void transformVect(Vector3f& vect) const;

    //! Transforms input vector by this matrix and stores result in output vector
    void transformVect(Vector3f& out, const Vector3f& in) const;

    //! An alternate transform vector method, writing into an array of 4 floats
    void transformVect(T* out,const Vector3f& in) const;

    //! Translate a vector by the translation part of this matrix.
    void translateVect(Vector3f& vect) const;

    //! Transforms a plane by this matrix
    void transformPlane(Plane3f& plane) const;

    //! Transforms a plane by this matrix
    void transformPlane(const Plane3f& in, Plane3f& out) const;

    //! Transforms a axis aligned bounding box
    /** The result box of this operation may not be accurate at all. For
    corRectangle results, use transformBoxEx() */
    void transformBox(BoundingBox3f& box) const;

    //! Transforms a axis aligned bounding box
    /** The result box of this operation should by accurate, but this operation
    is slower than transformBox(). */
    void transformBoxEx(BoundingBox3f& box) const;

    //! Multiplies this matrix by a 1x4 matrix
    void multiplyWith1x4Matrix(T* matrix) const;

    //! Calculates inverse of matrix. Slow.
    /** \return Returns false if there is no inverse matrix.*/
    bool makeInverse();

    //! Inverts a primitive matrix which only contains a translation and a rotation
    /** \param out: where result matrix is written to. */
    bool getInversePrimitive(CMatrix4<T>& out) const;

    //! Gets the inversed matrix of this one
    /** \param out: where result matrix is written to.
    \return Returns false if there is no inverse matrix. */
    bool getInverse(CMatrix4<T>& out) const;

    //! Builds a right-handed perspective projection matrix based on a field of view
    CMatrix4<T>& buildProjectionMatrixPerspectiveFovRH(float fieldOfViewRadians, float aspectRatio, float zNear, float zFar);

    //! Builds a left-handed perspective projection matrix based on a field of view
    CMatrix4<T>& buildProjectionMatrixPerspectiveFovLH(float fieldOfViewRadians, float aspectRatio, float zNear, float zFar);

    //! Builds a right-handed perspective projection matrix.
    CMatrix4<T>& buildProjectionMatrixPerspectiveRH(float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar);

    //! Builds a left-handed perspective projection matrix.
    CMatrix4<T>& buildProjectionMatrixPerspectiveLH(float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar);

    //! Builds a left-handed orthogonal projection matrix.
    CMatrix4<T>& buildProjectionMatrixOrthoLH(float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar);

    //! Builds a right-handed orthogonal projection matrix.
    CMatrix4<T>& buildProjectionMatrixOrthoRH(float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar);

    //! Builds a left-handed look-at matrix.
    CMatrix4<T>& buildCameraLookAtMatrixLH(
        const Vector3f& position,
        const Vector3f& target,
        const Vector3f& upVector);

    //! Builds a right-handed look-at matrix.
    CMatrix4<T>& buildCameraLookAtMatrixRH(
        const Vector3f& position,
        const Vector3f& target,
        const Vector3f& upVector);

    //! Builds a matrix that flattens geometry into a plane.
    /** \param light: light source
    \param plane: plane into which the geometry if flattened into
    \param point: value between 0 and 1, describing the light source.
    If this is 1, it is a point light, if it is 0, it is a diRectangleional light. */
    CMatrix4<T>& buildShadowMatrix(const Vector3f& light, Plane3f plane, float point=1.0f);

    //! Builds a matrix which transforms a normalized Device Coordinate to Device Coordinates.
    /** Used to scale <-1,-1><1,1> to viewport, for example from von <-1,-1> <1,1> to the viewport <0,0><0,640> */
    CMatrix4<T>& buildNDCToDCMatrix(const Rectanglei& area, float zScale);

    //! Creates a new matrix as interpolated matrix from two other ones.
    /** \param b: other matrix to interpolate with
    \param time: Must be a value between 0 and 1. */
    CMatrix4<T> interpolate(const CMatrix4<T>& b, float time) const;

    //! Gets transposed matrix
    CMatrix4<T> getTransposed() const;

    //! Gets transposed matrix
    inline void getTransposed(CMatrix4<T>& dest) const;

    //! Builds a matrix that rotates from one vector to another
    /** \param from: vector to rotate from
    \param to: vector to rotate to
     */
    CMatrix4<T>& buildRotateFromTo(const Vector3f& from, const Vector3f& to);

    //! Builds a combined matrix which translates to a center before rotation and translates from origin afterwards
    /** \param center Position to rotate around
    \param translate Translation applied after the rotation
     */
    void setRotationCenter(const Vector3f& center, const Vector3f& translate);

    //! Builds a matrix which rotates a source vector to a look vector over an arbitrary axis
    /** \param camPos: viewer position in world coo
    \param center: object position in world-coo and rotation pivot
    \param translation: object final translation from center
    \param axis: axis to rotate about
    \param from: source vector to rotate from
     */
    void buildAxisAlignedBillboard(const Vector3f& camPos,
                                   const Vector3f& center,
                                   const Vector3f& translation,
                                   const Vector3f& axis,
                                   const Vector3f& from);

    /*
    	construct 2D Texture transformations
    	rotate about center, scale, and transform.
    */
    //! Set to a texture transformation matrix with the given parameters.
    CMatrix4<T>& buildTextureTransform(float rotateRad,
                                       const Vector2f& rotatecenter,
                                       const Vector2f& translate,
                                       const Vector2f& scale);

    //! Set texture transformation rotation
    /** Rotate about z axis, recenter at (0.5,0.5).
    Doesn't clear other elements than those affected
    \param radAngle Angle in radians
    \return Altered matrix */
    CMatrix4<T>& setTextureRotationCenter(float radAngle);

    //! Set texture transformation translation
    /** Doesn't clear other elements than those affected.
    \param x Offset on x axis
    \param y Offset on y axis
    \return Altered matrix */
    CMatrix4<T>& setTextureTranslate(float x, float y);

    //! Set texture transformation translation, using a transposed representation
    /** Doesn't clear other elements than those affected.
    \param x Offset on x axis
    \param y Offset on y axis
    \return Altered matrix */
    CMatrix4<T>& setTextureTranslateTransposed(float x, float y);

    //! Set texture transformation scale
    /** Doesn't clear other elements than those affected.
    \param sx Scale factor on x axis
    \param sy Scale factor on y axis
    \return Altered matrix. */
    CMatrix4<T>& setTextureScale(float sx, float sy);

    //! Set texture transformation scale, and recenter at (0.5,0.5)
    /** Doesn't clear other elements than those affected.
    \param sx Scale factor on x axis
    \param sy Scale factor on y axis
    \return Altered matrix. */
    CMatrix4<T>& setTextureScaleCenter(float sx, float sy);

    //! Sets all matrix data members at once
    CMatrix4<T>& setM(const T* data);

    //! Gets if the matrix is definitely identity matrix
    bool getDefinitelyIdentityMatrix() const;

    //! Compare two matrices using the equal method
    bool equals(const CMatrix4<T>& other, const T tolerance=(T)Myth::Math::ROUNDING_ERROR_64) const;

private:
    //! Matrix data, stored in row-major order
    T M[16];
};

// Default constructor
template <class T>
inline CMatrix4<T>::CMatrix4(eConstructor constructor)
{
    switch(constructor)
    {
    case EM4CONST_NOTHING:
    case EM4CONST_COPY:
        break;
    case EM4CONST_IDENTITY:
    case EM4CONST_INVERSE:
    default:
        makeIdentity();
        break;
    }
}

// Copy constructor
template <class T>
inline CMatrix4<T>::CMatrix4(const CMatrix4<T>& other, eConstructor constructor)
{
    switch(constructor)
    {
    case EM4CONST_IDENTITY:
        makeIdentity();
        break;
    case EM4CONST_NOTHING:
        break;
    case EM4CONST_COPY:
        *this = other;
        break;
    case EM4CONST_TRANSPOSED:
        other.getTransposed(*this);
        break;
    case EM4CONST_INVERSE:
        if(!other.getInverse(*this))
            memset(M, 0, 16*sizeof(T));
        break;
    case EM4CONST_INVERSE_TRANSPOSED:
        if(!other.getInverse(*this))
            memset(M, 0, 16*sizeof(T));
        else
            *this=getTransposed();
        break;
    }
}

//! Add another matrix.
template <class T>
inline CMatrix4<T> CMatrix4<T>::operator+(const CMatrix4<T>& other) const
{
    CMatrix4<T> temp(EM4CONST_NOTHING);

    temp[0] = M[0]+other[0];
    temp[1] = M[1]+other[1];
    temp[2] = M[2]+other[2];
    temp[3] = M[3]+other[3];
    temp[4] = M[4]+other[4];
    temp[5] = M[5]+other[5];
    temp[6] = M[6]+other[6];
    temp[7] = M[7]+other[7];
    temp[8] = M[8]+other[8];
    temp[9] = M[9]+other[9];
    temp[10] = M[10]+other[10];
    temp[11] = M[11]+other[11];
    temp[12] = M[12]+other[12];
    temp[13] = M[13]+other[13];
    temp[14] = M[14]+other[14];
    temp[15] = M[15]+other[15];

    return temp;
}

//! Add another matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::operator+=(const CMatrix4<T>& other)
{
    M[0]+=other[0];
    M[1]+=other[1];
    M[2]+=other[2];
    M[3]+=other[3];
    M[4]+=other[4];
    M[5]+=other[5];
    M[6]+=other[6];
    M[7]+=other[7];
    M[8]+=other[8];
    M[9]+=other[9];
    M[10]+=other[10];
    M[11]+=other[11];
    M[12]+=other[12];
    M[13]+=other[13];
    M[14]+=other[14];
    M[15]+=other[15];

    return *this;
}

//! Subtract another matrix.
template <class T>
inline CMatrix4<T> CMatrix4<T>::operator-(const CMatrix4<T>& other) const
{
    CMatrix4<T> temp(EM4CONST_NOTHING);

    temp[0] = M[0]-other[0];
    temp[1] = M[1]-other[1];
    temp[2] = M[2]-other[2];
    temp[3] = M[3]-other[3];
    temp[4] = M[4]-other[4];
    temp[5] = M[5]-other[5];
    temp[6] = M[6]-other[6];
    temp[7] = M[7]-other[7];
    temp[8] = M[8]-other[8];
    temp[9] = M[9]-other[9];
    temp[10] = M[10]-other[10];
    temp[11] = M[11]-other[11];
    temp[12] = M[12]-other[12];
    temp[13] = M[13]-other[13];
    temp[14] = M[14]-other[14];
    temp[15] = M[15]-other[15];

    return temp;
}

//! Subtract another matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::operator-=(const CMatrix4<T>& other)
{
    M[0]-=other[0];
    M[1]-=other[1];
    M[2]-=other[2];
    M[3]-=other[3];
    M[4]-=other[4];
    M[5]-=other[5];
    M[6]-=other[6];
    M[7]-=other[7];
    M[8]-=other[8];
    M[9]-=other[9];
    M[10]-=other[10];
    M[11]-=other[11];
    M[12]-=other[12];
    M[13]-=other[13];
    M[14]-=other[14];
    M[15]-=other[15];

    return *this;
}

//! Multiply by scalar.
template <class T>
inline CMatrix4<T> CMatrix4<T>::operator*(const T& scalar) const
{
    CMatrix4<T> temp(EM4CONST_NOTHING);

    temp[0] = M[0]*scalar;
    temp[1] = M[1]*scalar;
    temp[2] = M[2]*scalar;
    temp[3] = M[3]*scalar;
    temp[4] = M[4]*scalar;
    temp[5] = M[5]*scalar;
    temp[6] = M[6]*scalar;
    temp[7] = M[7]*scalar;
    temp[8] = M[8]*scalar;
    temp[9] = M[9]*scalar;
    temp[10] = M[10]*scalar;
    temp[11] = M[11]*scalar;
    temp[12] = M[12]*scalar;
    temp[13] = M[13]*scalar;
    temp[14] = M[14]*scalar;
    temp[15] = M[15]*scalar;

    return temp;
}

//! Multiply by scalar.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::operator*=(const T& scalar)
{
    M[0]*=scalar;
    M[1]*=scalar;
    M[2]*=scalar;
    M[3]*=scalar;
    M[4]*=scalar;
    M[5]*=scalar;
    M[6]*=scalar;
    M[7]*=scalar;
    M[8]*=scalar;
    M[9]*=scalar;
    M[10]*=scalar;
    M[11]*=scalar;
    M[12]*=scalar;
    M[13]*=scalar;
    M[14]*=scalar;
    M[15]*=scalar;

    return *this;
}

//! Multiply by another matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::operator*=(const CMatrix4<T>& other)
{
    CMatrix4<T> temp(*this);
    return setbyproduct_nocheck(temp, other);
}

//! multiply by another matrix
// set this matrix to the product of two other matrices
// goal is to reduce stack use and copy
template <class T>
inline CMatrix4<T>& CMatrix4<T>::setbyproduct_nocheck(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b)
{
    const T* m1 = other_a.M;
    const T* m2 = other_b.M;

    M[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
    M[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
    M[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
    M[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];

    M[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
    M[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
    M[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
    M[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];

    M[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
    M[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
    M[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
    M[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];

    M[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
    M[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
    M[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
    M[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
    return *this;
}

//! multiply by another matrix
// set this matrix to the product of two other matrices
// goal is to reduce stack use and copy
template <class T>
inline CMatrix4<T>& CMatrix4<T>::setbyproduct(const CMatrix4<T>& other_a, const CMatrix4<T>& other_b)
{
    return setbyproduct_nocheck(other_a,other_b);
}

//! multiply by another matrix
template <class T>
inline CMatrix4<T> CMatrix4<T>::operator*(const CMatrix4<T>& m2) const
{
    CMatrix4<T> m3(EM4CONST_NOTHING);

    const T* m1 = M;

    m3[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
    m3[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
    m3[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
    m3[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];

    m3[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
    m3[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
    m3[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
    m3[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];

    m3[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
    m3[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
    m3[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
    m3[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];

    m3[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
    m3[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
    m3[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
    m3[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
    return m3;
}

template <class T>
inline Vector3<T> CMatrix4<T>::getTranslation() const
{
    return Vector3<T>(M[12], M[13], M[14]);
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setTranslation(const Vector3<T>& translation)
{
    M[12] = translation.X;
    M[13] = translation.Y;
    M[14] = translation.Z;
    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setInverseTranslation(const Vector3<T>& translation)
{
    M[12] = -translation.X;
    M[13] = -translation.Y;
    M[14] = -translation.Z;
    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setScale(const Vector3<T>& scale)
{
    M[0] = scale.X;
    M[5] = scale.Y;
    M[10] = scale.Z;
    return *this;
}

//! Returns the absolute values of the scales of the matrix.
/**
Note that this always returns the absolute (positive) values.  Unfortunately it
does not appear to be possible to extract any original negative values.  The best
that we could do would be to arbitrarily make one scale negative if one or three
of them were negative.
FIXME - return the original values.
*/
template <class T>
inline Vector3<T> CMatrix4<T>::getScale() const
{
    // See http://www.robertblum.com/articles/2005/02/14/decomposing-matrices

    // Deal with the 0 rotation case first
    // Prior to Irrlicht 1.6, we always returned this value.
    if(Myth::Math::is_zero(M[1]) && Myth::Math::is_zero(M[2]) &&
            Myth::Math::is_zero(M[4]) && Myth::Math::is_zero(M[6]) &&
            Myth::Math::is_zero(M[8]) && Myth::Math::is_zero(M[9]))
        return Vector3<T>(M[0], M[5], M[10]);

    // We have to do the full calculation.
    return Vector3<T>(sqrtf(M[0] * M[0] + M[1] * M[1] + M[2] * M[2]),
                      sqrtf(M[4] * M[4] + M[5] * M[5] + M[6] * M[6]),
                      sqrtf(M[8] * M[8] + M[9] * M[9] + M[10] * M[10]));
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setRotationDegrees(const Vector3<T>& rotation)
{
    return setRotationRadians(rotation * Myth::Math::DEGTORAD);
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setInverseRotationDegrees(const Vector3<T>& rotation)
{
    return setInverseRotationRadians(rotation * Myth::Math::DEGTORAD);
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setRotationRadians(const Vector3<T>& rotation)
{
    const double cr = cos(rotation.X);
    const double sr = sin(rotation.X);
    const double cp = cos(rotation.Y);
    const double sp = sin(rotation.Y);
    const double cy = cos(rotation.Z);
    const double sy = sin(rotation.Z);

    M[0] = (T)(cp*cy);
    M[1] = (T)(cp*sy);
    M[2] = (T)(-sp);

    const double srsp = sr*sp;
    const double crsp = cr*sp;

    M[4] = (T)(srsp*cy-cr*sy);
    M[5] = (T)(srsp*sy+cr*cy);
    M[6] = (T)(sr*cp);

    M[8] = (T)(crsp*cy+sr*sy);
    M[9] = (T)(crsp*sy-sr*cy);
    M[10] = (T)(cr*cp);
    return *this;
}

//! Returns a rotation that is equivalent to that set by setRotationDegrees().
/** This code was sent in by Chev.  Note that it does not necessarily return
the *same* Euler angles as those set by setRotationDegrees(), but the rotation will
be equivalent, i.e. will have the same result when used to rotate a vector or node. */
template <class T>
inline Vector3<T> CMatrix4<T>::getRotationDegrees() const
{
    const CMatrix4<T> &mat = *this;
    const Vector3<T> scale = getScale();
    const Vector3<double> invScale(Myth::Math::reciprocal(scale.X),Myth::Math::reciprocal(scale.Y),Myth::Math::reciprocal(scale.Z));

    double Y = -asin(mat[2]*invScale.X);
    const double C = cos(Y);
    Y *= Myth::Math::RADTODEG64;

    double rotx, roty, X, Z;

    if(!Myth::Math::is_zero(C))
    {
        const double invC = Myth::Math::reciprocal(C);
        rotx = mat[10] * invC * invScale.Z;
        roty = mat[6] * invC * invScale.Y;
        X = atan2(roty, rotx) * Myth::Math::RADTODEG64;
        rotx = mat[0] * invC * invScale.X;
        roty = mat[1] * invC * invScale.X;
        Z = atan2(roty, rotx) * Myth::Math::RADTODEG64;
    }
    else
    {
        X = 0.0;
        rotx = mat[5] * invScale.Y;
        roty = -mat[4] * invScale.Y;
        Z = atan2(roty, rotx) * Myth::Math::RADTODEG64;
    }

    // fix values that get below zero
    // before it would set (!) values to 360
    // that were above 360:
    if(X < 0.0) X += 360.0;
    if(Y < 0.0) Y += 360.0;
    if(Z < 0.0) Z += 360.0;

    return Vector3<T>((T)X,(T)Y,(T)Z);
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setInverseRotationRadians(const Vector3<T>& rotation)
{
    double cr = cos(rotation.X);
    double sr = sin(rotation.X);
    double cp = cos(rotation.Y);
    double sp = sin(rotation.Y);
    double cy = cos(rotation.Z);
    double sy = sin(rotation.Z);

    M[0] = (T)(cp*cy);
    M[4] = (T)(cp*sy);
    M[8] = (T)(-sp);

    double srsp = sr*sp;
    double crsp = cr*sp;

    M[1] = (T)(srsp*cy-cr*sy);
    M[5] = (T)(srsp*sy+cr*cy);
    M[9] = (T)(sr*cp);

    M[2] = (T)(crsp*cy+sr*sy);
    M[6] = (T)(crsp*sy-sr*cy);
    M[10] = (T)(cr*cp);
    return *this;
}

/*!
*/
template <class T>
inline CMatrix4<T>& CMatrix4<T>::makeIdentity()
{
    memset(M, 0, 16*sizeof(T));
    M[0] = M[5] = M[10] = M[15] = (T)1;
    return *this;
}

/*
	check identity with epsilon
	solve floating range problems..
*/
template <class T>
inline bool CMatrix4<T>::isIdentity() const
{
    if(!Myth::Math::equals(M[ 0], (T)1) ||
            !Myth::Math::equals(M[ 5], (T)1) ||
            !Myth::Math::equals(M[10], (T)1) ||
            !Myth::Math::equals(M[15], (T)1))
        return false;

    for(int32_t i=0; i<4; ++i)
        for(int32_t j=0; j<4; ++j)
            if((j != i) && (!Myth::Math::is_zero((*this)(i,j))))
                return false;

    return true;
}

/* Check orthogonality of matrix. */
template <class T>
inline bool CMatrix4<T>::isOrthogonal() const
{
    T dp=M[0] * M[4 ] + M[1] * M[5 ] + M[2 ] * M[6 ] + M[3 ] * M[7 ];
    if(!iszero(dp))
        return false;
    dp = M[0] * M[8 ] + M[1] * M[9 ] + M[2 ] * M[10] + M[3 ] * M[11];
    if(!iszero(dp))
        return false;
    dp = M[0] * M[12] + M[1] * M[13] + M[2 ] * M[14] + M[3 ] * M[15];
    if(!iszero(dp))
        return false;
    dp = M[4] * M[8 ] + M[5] * M[9 ] + M[6 ] * M[10] + M[7 ] * M[11];
    if(!iszero(dp))
        return false;
    dp = M[4] * M[12] + M[5] * M[13] + M[6 ] * M[14] + M[7 ] * M[15];
    if(!iszero(dp))
        return false;
    dp = M[8] * M[12] + M[9] * M[13] + M[10] * M[14] + M[11] * M[15];
    return (iszero(dp));
}

/*
	doesn't solve floating range problems..
	but takes care on +/- 0 on translation because we are changing it..
	reducing floating point branches
	but it needs the floats in memory..
*/
template <class T>
inline bool CMatrix4<T>::isIdentity_integer_base() const
{
    if(M[0] != (T)1)
        return false;
    if(M[1] != (T)0)
        return false;
    if(M[2] != (T)0)
        return false;
    if(M[3] != (T)0)
        return false;

    if(M[4] != (T)0)
        return false;
    if(M[5] != (T)1)
        return false;
    if(M[6] != (T)0)
        return false;
    if(M[7] != (T)0)
        return false;

    if(M[8] != (T)0)
        return false;
    if(M[9] != (T)0)
        return false;
    if(M[10] != (T)1)
        return false;
    if(M[11] != (T)0)
        return false;

    if(M[12] != (T)0)
        return false;
    if(M[13] != (T)0)
        return false;
    if(M[13] != (T)0)
        return false;
    if(M[15] != (T)1)
        return false;

    return true;
}

template <class T>
inline void CMatrix4<T>::rotateVect(Vector3f& vect) const
{
    Vector3f tmp = vect;
    vect.X = tmp.X*M[0] + tmp.Y*M[4] + tmp.Z*M[8];
    vect.Y = tmp.X*M[1] + tmp.Y*M[5] + tmp.Z*M[9];
    vect.Z = tmp.X*M[2] + tmp.Y*M[6] + tmp.Z*M[10];
}

//! An alternate transform vector method, writing into a second vector
template <class T>
inline void CMatrix4<T>::rotateVect(Vector3f& out, const Vector3f& in) const
{
    out.X = in.X*M[0] + in.Y*M[4] + in.Z*M[8];
    out.Y = in.X*M[1] + in.Y*M[5] + in.Z*M[9];
    out.Z = in.X*M[2] + in.Y*M[6] + in.Z*M[10];
}

//! An alternate transform vector method, writing into an array of 3 floats
template <class T>
inline void CMatrix4<T>::rotateVect(T* out, const Vector3f& in) const
{
    out[0] = in.X*M[0] + in.Y*M[4] + in.Z*M[8];
    out[1] = in.X*M[1] + in.Y*M[5] + in.Z*M[9];
    out[2] = in.X*M[2] + in.Y*M[6] + in.Z*M[10];
}

template <class T>
inline void CMatrix4<T>::inverseRotateVect(Vector3f& vect) const
{
    Vector3f tmp = vect;
    vect.X = tmp.X*M[0] + tmp.Y*M[1] + tmp.Z*M[2];
    vect.Y = tmp.X*M[4] + tmp.Y*M[5] + tmp.Z*M[6];
    vect.Z = tmp.X*M[8] + tmp.Y*M[9] + tmp.Z*M[10];
}

template <class T>
inline void CMatrix4<T>::transformVect(Vector3f& vect) const
{
    float vector[3];

    vector[0] = vect.X*M[0] + vect.Y*M[4] + vect.Z*M[8] + M[12];
    vector[1] = vect.X*M[1] + vect.Y*M[5] + vect.Z*M[9] + M[13];
    vector[2] = vect.X*M[2] + vect.Y*M[6] + vect.Z*M[10] + M[14];

    vect.X = vector[0];
    vect.Y = vector[1];
    vect.Z = vector[2];
}

template <class T>
inline void CMatrix4<T>::transformVect(Vector3f& out, const Vector3f& in) const
{
    out.X = in.X*M[0] + in.Y*M[4] + in.Z*M[8] + M[12];
    out.Y = in.X*M[1] + in.Y*M[5] + in.Z*M[9] + M[13];
    out.Z = in.X*M[2] + in.Y*M[6] + in.Z*M[10] + M[14];
}

template <class T>
inline void CMatrix4<T>::transformVect(T* out, const Vector3f& in) const
{
    out[0] = in.X*M[0] + in.Y*M[4] + in.Z*M[8] + M[12];
    out[1] = in.X*M[1] + in.Y*M[5] + in.Z*M[9] + M[13];
    out[2] = in.X*M[2] + in.Y*M[6] + in.Z*M[10] + M[14];
    out[3] = in.X*M[3] + in.Y*M[7] + in.Z*M[11] + M[15];
}

//! Transforms a plane by this matrix
template <class T>
inline void CMatrix4<T>::transformPlane(Plane3f& plane) const
{
    Vector3f member;
    // Transform the plane member point, i.e. rotate, translate and scale it.
    transformVect(member, plane.get_member_point());

    // Transform the normal by the transposed inverse of the matrix
    CMatrix4<T> transposedInverse(*this, EM4CONST_INVERSE_TRANSPOSED);
    Vector3f normal = plane.mNormal;
    transposedInverse.transformVect(normal);

    plane.set_plane(member, normal);
}

//! Transforms a plane by this matrix
template <class T>
inline void CMatrix4<T>::transformPlane(const Plane3f& in, Plane3f& out) const
{
    out = in;
    transformPlane(out);
}

//! Transforms a axis aligned bounding box
template <class T>
inline void CMatrix4<T>::transformBox(BoundingBox3f& box) const
{
    transformVect(box.MinEdge);
    transformVect(box.MaxEdge);
    box.repair();
}

//! Transforms a axis aligned bounding box more accurately than transformBox()
template <class T>
inline void CMatrix4<T>::transformBoxEx(BoundingBox3f& box) const
{
    const float Amin[3] = {box.MinEdge.X, box.MinEdge.Y, box.MinEdge.Z};
    const float Amax[3] = {box.MaxEdge.X, box.MaxEdge.Y, box.MaxEdge.Z};

    float Bmin[3];
    float Bmax[3];

    Bmin[0] = Bmax[0] = M[12];
    Bmin[1] = Bmax[1] = M[13];
    Bmin[2] = Bmax[2] = M[14];

    const CMatrix4<T> &m = *this;

    for(uint32_t i = 0; i < 3; ++i)
    {
        for(uint32_t j = 0; j < 3; ++j)
        {
            const float a = m(j,i) * Amin[j];
            const float b = m(j,i) * Amax[j];

            if(a < b)
            {
                Bmin[i] += a;
                Bmax[i] += b;
            }
            else
            {
                Bmin[i] += b;
                Bmax[i] += a;
            }
        }
    }

    box.MinEdge.X = Bmin[0];
    box.MinEdge.Y = Bmin[1];
    box.MinEdge.Z = Bmin[2];

    box.MaxEdge.X = Bmax[0];
    box.MaxEdge.Y = Bmax[1];
    box.MaxEdge.Z = Bmax[2];
}

//! Multiplies this matrix by a 1x4 matrix
template <class T>
inline void CMatrix4<T>::multiplyWith1x4Matrix(T* matrix) const
{
    /*
    0  1  2  3
    4  5  6  7
    8  9  10 11
    12 13 14 15
    */

    T mat[4];
    mat[0] = matrix[0];
    mat[1] = matrix[1];
    mat[2] = matrix[2];
    mat[3] = matrix[3];

    matrix[0] = M[0]*mat[0] + M[4]*mat[1] + M[8]*mat[2] + M[12]*mat[3];
    matrix[1] = M[1]*mat[0] + M[5]*mat[1] + M[9]*mat[2] + M[13]*mat[3];
    matrix[2] = M[2]*mat[0] + M[6]*mat[1] + M[10]*mat[2] + M[14]*mat[3];
    matrix[3] = M[3]*mat[0] + M[7]*mat[1] + M[11]*mat[2] + M[15]*mat[3];
}

template <class T>
inline void CMatrix4<T>::inverseTranslateVect(Vector3f& vect) const
{
    vect.X = vect.X-M[12];
    vect.Y = vect.Y-M[13];
    vect.Z = vect.Z-M[14];
}

template <class T>
inline void CMatrix4<T>::translateVect(Vector3f& vect) const
{
    vect.X = vect.X+M[12];
    vect.Y = vect.Y+M[13];
    vect.Z = vect.Z+M[14];
}

template <class T>
inline bool CMatrix4<T>::getInverse(CMatrix4<T>& out) const
{
    /// Calculates the inverse of this Matrix
    /// The inverse is calculated using Cramers rule.
    /// If no inverse exists then 'false' is returned.

    const CMatrix4<T> &m = *this;

    float d = (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) * (m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2)) -
              (m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) * (m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1)) +
              (m(0, 0) * m(1, 3) - m(0, 3) * m(1, 0)) * (m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1)) +
              (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * (m(2, 0) * m(3, 3) - m(2, 3) * m(3, 0)) -
              (m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1)) * (m(2, 0) * m(3, 2) - m(2, 2) * m(3, 0)) +
              (m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2)) * (m(2, 0) * m(3, 1) - m(2, 1) * m(3, 0));

    if(Myth::Math::is_zero(d))
        return false;

    d = Myth::Math::reciprocal(d);

    out(0, 0) = d * (m(1, 1) * (m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2)) +
                     m(1, 2) * (m(2, 3) * m(3, 1) - m(2, 1) * m(3, 3)) +
                     m(1, 3) * (m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1)));
    out(0, 1) = d * (m(2, 1) * (m(0, 2) * m(3, 3) - m(0, 3) * m(3, 2)) +
                     m(2, 2) * (m(0, 3) * m(3, 1) - m(0, 1) * m(3, 3)) +
                     m(2, 3) * (m(0, 1) * m(3, 2) - m(0, 2) * m(3, 1)));
    out(0, 2) = d * (m(3, 1) * (m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2)) +
                     m(3, 2) * (m(0, 3) * m(1, 1) - m(0, 1) * m(1, 3)) +
                     m(3, 3) * (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)));
    out(0, 3) = d * (m(0, 1) * (m(1, 3) * m(2, 2) - m(1, 2) * m(2, 3)) +
                     m(0, 2) * (m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1)) +
                     m(0, 3) * (m(1, 2) * m(2, 1) - m(1, 1) * m(2, 2)));
    out(1, 0) = d * (m(1, 2) * (m(2, 0) * m(3, 3) - m(2, 3) * m(3, 0)) +
                     m(1, 3) * (m(2, 2) * m(3, 0) - m(2, 0) * m(3, 2)) +
                     m(1, 0) * (m(2, 3) * m(3, 2) - m(2, 2) * m(3, 3)));
    out(1, 1) = d * (m(2, 2) * (m(0, 0) * m(3, 3) - m(0, 3) * m(3, 0)) +
                     m(2, 3) * (m(0, 2) * m(3, 0) - m(0, 0) * m(3, 2)) +
                     m(2, 0) * (m(0, 3) * m(3, 2) - m(0, 2) * m(3, 3)));
    out(1, 2) = d * (m(3, 2) * (m(0, 0) * m(1, 3) - m(0, 3) * m(1, 0)) +
                     m(3, 3) * (m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2)) +
                     m(3, 0) * (m(0, 3) * m(1, 2) - m(0, 2) * m(1, 3)));
    out(1, 3) = d * (m(0, 2) * (m(1, 3) * m(2, 0) - m(1, 0) * m(2, 3)) +
                     m(0, 3) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
                     m(0, 0) * (m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2)));
    out(2, 0) = d * (m(1, 3) * (m(2, 0) * m(3, 1) - m(2, 1) * m(3, 0)) +
                     m(1, 0) * (m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1)) +
                     m(1, 1) * (m(2, 3) * m(3, 0) - m(2, 0) * m(3, 3)));
    out(2, 1) = d * (m(2, 3) * (m(0, 0) * m(3, 1) - m(0, 1) * m(3, 0)) +
                     m(2, 0) * (m(0, 1) * m(3, 3) - m(0, 3) * m(3, 1)) +
                     m(2, 1) * (m(0, 3) * m(3, 0) - m(0, 0) * m(3, 3)));
    out(2, 2) = d * (m(3, 3) * (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) +
                     m(3, 0) * (m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1)) +
                     m(3, 1) * (m(0, 3) * m(1, 0) - m(0, 0) * m(1, 3)));
    out(2, 3) = d * (m(0, 3) * (m(1, 1) * m(2, 0) - m(1, 0) * m(2, 1)) +
                     m(0, 0) * (m(1, 3) * m(2, 1) - m(1, 1) * m(2, 3)) +
                     m(0, 1) * (m(1, 0) * m(2, 3) - m(1, 3) * m(2, 0)));
    out(3, 0) = d * (m(1, 0) * (m(2, 2) * m(3, 1) - m(2, 1) * m(3, 2)) +
                     m(1, 1) * (m(2, 0) * m(3, 2) - m(2, 2) * m(3, 0)) +
                     m(1, 2) * (m(2, 1) * m(3, 0) - m(2, 0) * m(3, 1)));
    out(3, 1) = d * (m(2, 0) * (m(0, 2) * m(3, 1) - m(0, 1) * m(3, 2)) +
                     m(2, 1) * (m(0, 0) * m(3, 2) - m(0, 2) * m(3, 0)) +
                     m(2, 2) * (m(0, 1) * m(3, 0) - m(0, 0) * m(3, 1)));
    out(3, 2) = d * (m(3, 0) * (m(0, 2) * m(1, 1) - m(0, 1) * m(1, 2)) +
                     m(3, 1) * (m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) +
                     m(3, 2) * (m(0, 1) * m(1, 0) - m(0, 0) * m(1, 1)));
    out(3, 3) = d * (m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) +
                     m(0, 1) * (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) +
                     m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)));

    return true;
}

//! Inverts a primitive matrix which only contains a translation and a rotation
//! \param out: where result matrix is written to.
template <class T>
inline bool CMatrix4<T>::getInversePrimitive(CMatrix4<T>& out) const
{
    out.M[0 ] = M[0];
    out.M[1 ] = M[4];
    out.M[2 ] = M[8];
    out.M[3 ] = 0;

    out.M[4 ] = M[1];
    out.M[5 ] = M[5];
    out.M[6 ] = M[9];
    out.M[7 ] = 0;

    out.M[8 ] = M[2];
    out.M[9 ] = M[6];
    out.M[10] = M[10];
    out.M[11] = 0;

    out.M[12] = (T)-(M[12]*M[0] + M[13]*M[1] + M[14]*M[2]);
    out.M[13] = (T)-(M[12]*M[4] + M[13]*M[5] + M[14]*M[6]);
    out.M[14] = (T)-(M[12]*M[8] + M[13]*M[9] + M[14]*M[10]);
    out.M[15] = 1;

    return true;
}

/*!
*/
template <class T>
inline bool CMatrix4<T>::makeInverse()
{
    CMatrix4<T> temp(EM4CONST_NOTHING);

    if(getInverse(temp))
    {
        *this = temp;
        return true;
    }

    return false;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::operator=(const CMatrix4<T> &other)
{
    if(this==&other)
        return *this;
    memcpy(M, other.M, 16*sizeof(T));
    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::operator=(const T& scalar)
{
    for(int32_t i = 0; i < 16; ++i)
        M[i]=scalar;

    return *this;
}

template <class T>
inline bool CMatrix4<T>::operator==(const CMatrix4<T> &other) const
{
    for(int32_t i = 0; i < 16; ++i)
        if(M[i] != other.M[i])
            return false;

    return true;
}

template <class T>
inline bool CMatrix4<T>::operator!=(const CMatrix4<T> &other) const
{
    return !(*this == other);
}

// Builds a right-handed perspective projection matrix based on a field of view
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovRH(
    float fieldOfViewRadians, float aspectRatio, float zNear, float zFar)
{
    const double h = Myth::Math::reciprocal(tan(fieldOfViewRadians*0.5));
    const T w = h / aspectRatio;

    M[0] = w;
    M[1] = 0;
    M[2] = 0;
    M[3] = 0;

    M[4] = 0;
    M[5] = (T)h;
    M[6] = 0;
    M[7] = 0;

    M[8] = 0;
    M[9] = 0;
    M[10] = (T)(zFar/(zNear-zFar)); // DiRectangleX version
//		M[10] = (T)(zFar+zNear/(zNear-zFar)); // OpenGL version
    M[11] = -1;

    M[12] = 0;
    M[13] = 0;
    M[14] = (T)(zNear*zFar/(zNear-zFar)); // DiRectangleX version
//		M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar)); // OpenGL version
    M[15] = 0;

    return *this;
}

// Builds a left-handed perspective projection matrix based on a field of view
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovLH(
    float fieldOfViewRadians, float aspectRatio, float zNear, float zFar)
{
    const double h = Myth::Math::reciprocal(tan(fieldOfViewRadians*0.5));
    const T w = (T)(h / aspectRatio);

    M[0] = w;
    M[1] = 0;
    M[2] = 0;
    M[3] = 0;

    M[4] = 0;
    M[5] = (T)h;
    M[6] = 0;
    M[7] = 0;

    M[8] = 0;
    M[9] = 0;
    M[10] = (T)(zFar/(zFar-zNear));
    M[11] = 1;

    M[12] = 0;
    M[13] = 0;
    M[14] = (T)(-zNear*zFar/(zFar-zNear));
    M[15] = 0;

    return *this;
}

// Builds a left-handed orthogonal projection matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixOrthoLH(
    float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar)
{
    M[0] = (T)(2/widthOfViewVolume);
    M[1] = 0;
    M[2] = 0;
    M[3] = 0;

    M[4] = 0;
    M[5] = (T)(2/heightOfViewVolume);
    M[6] = 0;
    M[7] = 0;

    M[8] = 0;
    M[9] = 0;
    M[10] = (T)(1/(zFar-zNear));
    M[11] = 0;

    M[12] = 0;
    M[13] = 0;
    M[14] = (T)(zNear/(zNear-zFar));
    M[15] = 1;

    return *this;
}

// Builds a right-handed orthogonal projection matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixOrthoRH(
    float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar)
{
    M[0] = (T)(2/widthOfViewVolume);
    M[1] = 0;
    M[2] = 0;
    M[3] = 0;

    M[4] = 0;
    M[5] = (T)(2/heightOfViewVolume);
    M[6] = 0;
    M[7] = 0;

    M[8] = 0;
    M[9] = 0;
    M[10] = (T)(1/(zNear-zFar));
    M[11] = 0;

    M[12] = 0;
    M[13] = 0;
    M[14] = (T)(zNear/(zNear-zFar));
    M[15] = -1;

    return *this;
}

// Builds a right-handed perspective projection matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveRH(
    float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar)
{
    M[0] = (T)(2*zNear/widthOfViewVolume);
    M[1] = 0;
    M[2] = 0;
    M[3] = 0;

    M[4] = 0;
    M[5] = (T)(2*zNear/heightOfViewVolume);
    M[6] = 0;
    M[7] = 0;

    M[8] = 0;
    M[9] = 0;
    M[10] = (T)(zFar/(zNear-zFar));
    M[11] = -1;

    M[12] = 0;
    M[13] = 0;
    M[14] = (T)(zNear*zFar/(zNear-zFar));
    M[15] = 0;

    return *this;
}

// Builds a left-handed perspective projection matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveLH(
    float widthOfViewVolume, float heightOfViewVolume, float zNear, float zFar)
{
    M[0] = (T)(2*zNear/widthOfViewVolume);
    M[1] = 0;
    M[2] = 0;
    M[3] = 0;

    M[4] = 0;
    M[5] = (T)(2*zNear/heightOfViewVolume);
    M[6] = 0;
    M[7] = 0;

    M[8] = 0;
    M[9] = 0;
    M[10] = (T)(zFar/(zFar-zNear));
    M[11] = 1;

    M[12] = 0;
    M[13] = 0;
    M[14] = (T)(zNear*zFar/(zNear-zFar));
    M[15] = 0;
    return *this;
}

// Builds a matrix that flattens geometry into a plane.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildShadowMatrix(const Vector3f& light, Plane3f plane, float point)
{
    plane.mNormal.normalize();
    const float d = plane.mNormal.dot_product(light);

    M[ 0] = (T)(-plane.mNormal.X * light.X + d);
    M[ 1] = (T)(-plane.mNormal.X * light.Y);
    M[ 2] = (T)(-plane.mNormal.X * light.Z);
    M[ 3] = (T)(-plane.mNormal.X * point);

    M[ 4] = (T)(-plane.mNormal.Y * light.X);
    M[ 5] = (T)(-plane.mNormal.Y * light.Y + d);
    M[ 6] = (T)(-plane.mNormal.Y * light.Z);
    M[ 7] = (T)(-plane.mNormal.Y * point);

    M[ 8] = (T)(-plane.mNormal.Z * light.X);
    M[ 9] = (T)(-plane.mNormal.Z * light.Y);
    M[10] = (T)(-plane.mNormal.Z * light.Z + d);
    M[11] = (T)(-plane.mNormal.Z * point);

    M[12] = (T)(-plane.mD * light.X);
    M[13] = (T)(-plane.mD * light.Y);
    M[14] = (T)(-plane.mD * light.Z);
    M[15] = (T)(-plane.mD * point + d);
    return *this;
}

// Builds a left-handed look-at matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildCameraLookAtMatrixLH(
    const Vector3f& position,
    const Vector3f& target,
    const Vector3f& upVector)
{
    Vector3f zaxis = target - position;
    zaxis.normalize();

    Vector3f xaxis = upVector.cross_product(zaxis);
    xaxis.normalize();

    Vector3f yaxis = zaxis.cross_product(xaxis);

    M[0] = (T)xaxis.X;
    M[1] = (T)yaxis.X;
    M[2] = (T)zaxis.X;
    M[3] = 0;

    M[4] = (T)xaxis.Y;
    M[5] = (T)yaxis.Y;
    M[6] = (T)zaxis.Y;
    M[7] = 0;

    M[8] = (T)xaxis.Z;
    M[9] = (T)yaxis.Z;
    M[10] = (T)zaxis.Z;
    M[11] = 0;

    M[12] = (T)-xaxis.dot_product(position);
    M[13] = (T)-yaxis.dot_product(position);
    M[14] = (T)-zaxis.dot_product(position);
    M[15] = 1;
    return *this;
}

// Builds a right-handed look-at matrix.
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildCameraLookAtMatrixRH(
    const Vector3f& position,
    const Vector3f& target,
    const Vector3f& upVector)
{
    Vector3f zaxis = position - target;
    zaxis.normalize();

    Vector3f xaxis = upVector.cross_product(zaxis);
    xaxis.normalize();

    Vector3f yaxis = zaxis.cross_product(xaxis);

    M[0] = (T)xaxis.X;
    M[1] = (T)yaxis.X;
    M[2] = (T)zaxis.X;
    M[3] = 0;

    M[4] = (T)xaxis.Y;
    M[5] = (T)yaxis.Y;
    M[6] = (T)zaxis.Y;
    M[7] = 0;

    M[8] = (T)xaxis.Z;
    M[9] = (T)yaxis.Z;
    M[10] = (T)zaxis.Z;
    M[11] = 0;

    M[12] = (T)-xaxis.dot_product(position);
    M[13] = (T)-yaxis.dot_product(position);
    M[14] = (T)-zaxis.dot_product(position);
    M[15] = 1;
    return *this;
}

// creates a new matrix as interpolated matrix from this and the passed one.
template <class T>
inline CMatrix4<T> CMatrix4<T>::interpolate(const CMatrix4<T>& b, float time) const
{
    CMatrix4<T> mat(EM4CONST_NOTHING);

    for(uint32_t i=0; i < 16; i += 4)
    {
        mat.M[i+0] = (T)(M[i+0] + (b.M[i+0] - M[i+0]) * time);
        mat.M[i+1] = (T)(M[i+1] + (b.M[i+1] - M[i+1]) * time);
        mat.M[i+2] = (T)(M[i+2] + (b.M[i+2] - M[i+2]) * time);
        mat.M[i+3] = (T)(M[i+3] + (b.M[i+3] - M[i+3]) * time);
    }
    return mat;
}

// returns transposed matrix
template <class T>
inline CMatrix4<T> CMatrix4<T>::getTransposed() const
{
    CMatrix4<T> t(EM4CONST_NOTHING);
    getTransposed(t);
    return t;
}

// returns transposed matrix
template <class T>
inline void CMatrix4<T>::getTransposed(CMatrix4<T>& o) const
{
    o[ 0] = M[ 0];
    o[ 1] = M[ 4];
    o[ 2] = M[ 8];
    o[ 3] = M[12];

    o[ 4] = M[ 1];
    o[ 5] = M[ 5];
    o[ 6] = M[ 9];
    o[ 7] = M[13];

    o[ 8] = M[ 2];
    o[ 9] = M[ 6];
    o[10] = M[10];
    o[11] = M[14];

    o[12] = M[ 3];
    o[13] = M[ 7];
    o[14] = M[11];
    o[15] = M[15];
}

// used to scale <-1,-1><1,1> to viewport
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildNDCToDCMatrix(const Rectanglei& viewport, float zScale)
{
    const float scaleX = (viewport.get_width() - 0.75f) * 0.5f;
    const float scaleY = -(viewport.get_height() - 0.75f) * 0.5f;

    const float dx = -0.5f + ((viewport.UpperLeftCorner.X + viewport.LowerRightCorner.X) * 0.5f);
    const float dy = -0.5f + ((viewport.UpperLeftCorner.Y + viewport.LowerRightCorner.Y) * 0.5f);

    makeIdentity();
    M[12] = (T)dx;
    M[13] = (T)dy;
    return setScale(Vector3<T>((T)scaleX, (T)scaleY, (T)zScale));
}

//! Builds a matrix that rotates from one vector to another
/** \param from: vector to rotate from
\param to: vector to rotate to

	http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
 */
template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildRotateFromTo(const Vector3f& from, const Vector3f& to)
{
    // unit vectors
    Vector3f f(from);
    Vector3f t(to);
    f.normalize();
    t.normalize();

    // axis multiplication by sin
    Vector3f vs(t.cross_product(f));

    // axis of rotation
    Vector3f v(vs);
    v.normalize();

    // cosinus angle
    T ca = f.dot_product(t);

    Vector3f vt(v *((T) 1 - ca));

    M[0] = vt.X * v.X + ca;
    M[5] = vt.Y * v.Y + ca;
    M[10] = vt.Z * v.Z + ca;

    vt.X *= v.Y;
    vt.Z *= v.X;
    vt.Y *= v.Z;

    M[1] = vt.X - vs.Z;
    M[2] = vt.Z + vs.Y;
    M[3] = (T) 0;

    M[4] = vt.X + vs.Z;
    M[6] = vt.Y - vs.X;
    M[7] = (T) 0;

    M[8] = vt.Z - vs.Y;
    M[9] = vt.Y + vs.X;
    M[11] = (T) 0;

    M[12] = (T) 0;
    M[13] = (T) 0;
    M[14] = (T) 0;
    M[15] = (T) 1;

    return *this;
}

//! Builds a matrix which rotates a source vector to a look vector over an arbitrary axis
/** \param camPos: viewer position in world coo
\param center: object position in world-coo and rotation pivot
\param translation: object final translation from center
\param axis: axis to rotate about
\param from: source vector to rotate from
 */
template <class T>
inline void CMatrix4<T>::buildAxisAlignedBillboard(const Vector3f& camPos,
        const Vector3f& center,
        const Vector3f& translation,
        const Vector3f& axis,
        const Vector3f& from
                                                  )
{
    // axis of rotation
    Vector3f up = axis;
    up.normalize();

    Vector3f forward = camPos - center;
    forward.normalize();

    Vector3f right = up.cross_product(forward);
    right.normalize();

    // corRectangle look vector
    Vector3f look = right.cross_product(up);

    // rotate from to

    // axis multiplication by sin
    Vector3f vs = look.cross_product(from);

    // cosinus angle
    float ca = from.dot_product(look);

    Vector3f vt(up *(1.f - ca));

    M[0] = vt.X * up.X + ca;
    M[5] = vt.Y * up.Y + ca;
    M[10] = vt.Z * up.Z + ca;

    vt.X *= up.Y;
    vt.Z *= up.X;
    vt.Y *= up.Z;

    M[1] = vt.X - vs.Z;
    M[2] = vt.Z + vs.Y;
    M[3] = (T) 0;

    M[4] = vt.X + vs.Z;
    M[6] = vt.Y - vs.X;
    M[7] = (T) 0;

    M[8] = vt.Z - vs.Y;
    M[9] = vt.Y + vs.X;
    M[11] = (T) 0;

    setRotationCenter(center, translation);

}

//! Builds a combined matrix which translate to a center before rotation and translate afterwards
template <class T>
inline void CMatrix4<T>::setRotationCenter(const Vector3f& center, const Vector3f& translation)
{
    M[12] = -M[0]*center.X - M[4]*center.Y - M[8]*center.Z + (center.X - translation.X);
    M[13] = -M[1]*center.X - M[5]*center.Y - M[9]*center.Z + (center.Y - translation.Y);
    M[14] = -M[2]*center.X - M[6]*center.Y - M[10]*center.Z + (center.Z - translation.Z);
    M[15] = (T) 1.0;
}

/*!
	Generate texture coordinates as linear functions so that:
		u = Ux*x + Uy*y + Uz*z + Uw
		v = Vx*x + Vy*y + Vz*z + Vw
	The matrix M for this case is:
		Ux  Vx  0  0
		Uy  Vy  0  0
		Uz  Vz  0  0
		Uw  Vw  0  0
*/

template <class T>
inline CMatrix4<T>& CMatrix4<T>::buildTextureTransform(float rotateRad,
        const Vector2f& rotatecenter,
        const Vector2f& translate,
        const Vector2f& scale)
{
    const float c = cosf(rotateRad);
    const float s = sinf(rotateRad);

    M[0] = (T)(c * scale.X);
    M[1] = (T)(s * scale.Y);
    M[2] = 0;
    M[3] = 0;

    M[4] = (T)(-s * scale.X);
    M[5] = (T)(c * scale.Y);
    M[6] = 0;
    M[7] = 0;

    M[8] = (T)(c * scale.X * rotatecenter.X + -s * rotatecenter.Y + translate.X);
    M[9] = (T)(s * scale.Y * rotatecenter.X +  c * rotatecenter.Y + translate.Y);
    M[10] = 1;
    M[11] = 0;

    M[12] = 0;
    M[13] = 0;
    M[14] = 0;
    M[15] = 1;
    return *this;
}

// rotate about z axis, center ( 0.5, 0.5 )
template <class T>
inline CMatrix4<T>& CMatrix4<T>::setTextureRotationCenter(float rotateRad)
{
    const float c = cosf(rotateRad);
    const float s = sinf(rotateRad);
    M[0] = (T)c;
    M[1] = (T)s;

    M[4] = (T)-s;
    M[5] = (T)c;

    M[8] = (T)(0.5f * (s - c) + 0.5f);
    M[9] = (T)(-0.5f * (s + c) + 0.5f);

    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setTextureTranslate(float x, float y)
{
    M[8] = (T)x;
    M[9] = (T)y;

    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setTextureTranslateTransposed(float x, float y)
{
    M[2] = (T)x;
    M[6] = (T)y;

    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setTextureScale(float sx, float sy)
{
    M[0] = (T)sx;
    M[5] = (T)sy;
    return *this;
}

template <class T>
inline CMatrix4<T>& CMatrix4<T>::setTextureScaleCenter(float sx, float sy)
{
    M[0] = (T)sx;
    M[5] = (T)sy;
    M[8] = (T)(0.5f - 0.5f * sx);
    M[9] = (T)(0.5f - 0.5f * sy);

    return *this;
}

// sets all matrix data members at once
template <class T>
inline CMatrix4<T>& CMatrix4<T>::setM(const T* data)
{
    memcpy(M,data, 16*sizeof(T));

    return *this;
}

//! Compare two matrices using the equal method
template <class T>
inline bool CMatrix4<T>::equals(const CMatrix4<T>& other, const T tolerance) const
{
    for(int32_t i = 0; i < 16; ++i)
        if(!Myth::Math::equals(M[i],other.M[i], tolerance))
            return false;

    return true;
}

// Multiply by scalar.
template <class T>
inline CMatrix4<T> operator*(const T scalar, const CMatrix4<T>& mat)
{
    return mat*scalar;
}

//! Typedef for f32 matrix
typedef CMatrix4<float> Matrix4;

//! Quaternion class for representing rotations.
/** It provides cheap combinations and avoids gimbal locks.
Also useful for interpolations. */
class Quaternion
{
public:

    //! Default Constructor
    Quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}

    //! Constructor
    Quaternion(float x, float y, float z, float w) : X(x), Y(y), Z(z), W(w) { }

    //! Constructor which converts euler angles (radians) to a Quaternion
    Quaternion(float x, float y, float z);

    //! Constructor which converts euler angles (radians) to a Quaternion
    Quaternion(const Vector3f& vec);

    //! Constructor which converts a matrix to a Quaternion
    Quaternion(const Matrix4& mat);

    //! Equalilty operator
    bool operator==(const Quaternion& other) const;

    //! inequality operator
    bool operator!=(const Quaternion& other) const;

    //! Assignment operator
    inline Quaternion& operator=(const Quaternion& other);

    //! Matrix assignment operator
    inline Quaternion& operator=(const Matrix4& other);

    //! Add operator
    Quaternion operator+(const Quaternion& other) const;

    //! Multiplication operator
    Quaternion operator*(const Quaternion& other) const;

    //! Multiplication operator with scalar
    Quaternion operator*(float s) const;

    //! Multiplication operator with scalar
    Quaternion& operator*=(float s);

    //! Multiplication operator
    Vector3f operator*(const Vector3f& v) const;

    //! Multiplication operator
    Quaternion& operator*=(const Quaternion& other);

    //! Calculates the dot product
    inline float dotProduct(const Quaternion& other) const;

    //! Sets new Quaternion
    inline Quaternion& set(float x, float y, float z, float w);

    //! Sets new Quaternion based on euler angles (radians)
    inline Quaternion& set(float x, float y, float z);

    //! Sets new Quaternion based on euler angles (radians)
    inline Quaternion& set(const Vector3f& vec);

    //! Sets new Quaternion from other Quaternion
    inline Quaternion& set(const Quaternion& quat);

    //! returns if this Quaternion equals the other one, taking floating point rounding errors into account
    inline bool equals(const Quaternion& other, const float tolerance = Myth::Math::ROUNDING_ERROR_32) const;

    //! Normalizes the Quaternion
    inline Quaternion& normalize();

    //! Creates a matrix from this Quaternion
    Matrix4 getMatrix() const;

    //! Creates a matrix from this Quaternion
    void getMatrix(Matrix4& dest, const Vector3f& translation) const;

    /*!
    	Creates a matrix from this Quaternion
    	Rotate about a center point
    	shortcut for
    	core::Quaternion q;
    	q.rotationFromTo ( vin[i].Normal, forward );
    	q.getMatrixCenter ( lookat, center, newPos );

    	core::Matrix4 m2;
    	m2.setInverseTranslation ( center );
    	lookat *= m2;

    	core::Matrix4 m3;
    	m2.setTranslation ( newPos );
    	lookat *= m3;

    */
    void getMatrixCenter(Matrix4& dest, const Vector3f& center, const Vector3f& translation) const;

    //! Creates a matrix from this Quaternion
    inline void getMatrix_transposed(Matrix4& dest) const;

    //! Inverts this Quaternion
    Quaternion& makeInverse();

    //! Set this Quaternion to the result of the interpolation between two Quaternions
    Quaternion& slerp(Quaternion q1, Quaternion q2, float interpolate);

    //! Create Quaternion from rotation angle and rotation axis.
    /** Axis must be unit length.
    The Quaternion representing the rotation is
    q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k).
    \param angle Rotation Angle in radians.
    \param axis Rotation axis. */
    Quaternion& fromAngleAxis(float angle, const Vector3f& axis);

    //! Fills an angle (radians) around an axis (unit vector)
    void toAngleAxis(float& angle, Vector3f& axis) const;

    //! Output this Quaternion to an euler angle (radians)
    void toEuler(Vector3f& euler) const;

    //! Set Quaternion to identity
    Quaternion& makeIdentity();

    //! Set Quaternion to represent a rotation from one vector to another.
    Quaternion& rotationFromTo(const Vector3f& from, const Vector3f& to);

    //! Quaternion elements.
    float X; // vectorial (imaginary) part
    float Y;
    float Z;
    float W; // real part
};


// Constructor which converts euler angles to a Quaternion
inline Quaternion::Quaternion(float x, float y, float z)
{
    set(x,y,z);
}


// Constructor which converts euler angles to a Quaternion
inline Quaternion::Quaternion(const Vector3f& vec)
{
    set(vec.X,vec.Y,vec.Z);
}


// Constructor which converts a matrix to a Quaternion
inline Quaternion::Quaternion(const Matrix4& mat)
{
    (*this) = mat;
}


// equal operator
inline bool Quaternion::operator==(const Quaternion& other) const
{
    return ((X == other.X) &&
            (Y == other.Y) &&
            (Z == other.Z) &&
            (W == other.W));
}

// inequality operator
inline bool Quaternion::operator!=(const Quaternion& other) const
{
    return !(*this == other);
}

// assignment operator
inline Quaternion& Quaternion::operator=(const Quaternion& other)
{
    X = other.X;
    Y = other.Y;
    Z = other.Z;
    W = other.W;
    return *this;
}


// matrix assignment operator
inline Quaternion& Quaternion::operator=(const Matrix4& m)
{
    const float diag = m(0,0) + m(1,1) + m(2,2) + 1;

    if(diag > 0.0f)
    {
        const float scale = sqrtf(diag) * 2.0f; // get scale from diagonal

        // TODO: speed this up
        X = (m(2,1) - m(1,2)) / scale;
        Y = (m(0,2) - m(2,0)) / scale;
        Z = (m(1,0) - m(0,1)) / scale;
        W = 0.25f * scale;
    }
    else
    {
        if(m(0,0) > m(1,1) && m(0,0) > m(2,2))
        {
            // 1st element of diag is greatest value
            // find scale according to 1st element, and double it
            const float scale = sqrtf(1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;

            // TODO: speed this up
            X = 0.25f * scale;
            Y = (m(0,1) + m(1,0)) / scale;
            Z = (m(2,0) + m(0,2)) / scale;
            W = (m(2,1) - m(1,2)) / scale;
        }
        else if(m(1,1) > m(2,2))
        {
            // 2nd element of diag is greatest value
            // find scale according to 2nd element, and double it
            const float scale = sqrtf(1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;

            // TODO: speed this up
            X = (m(0,1) + m(1,0)) / scale;
            Y = 0.25f * scale;
            Z = (m(1,2) + m(2,1)) / scale;
            W = (m(0,2) - m(2,0)) / scale;
        }
        else
        {
            // 3rd element of diag is greatest value
            // find scale according to 3rd element, and double it
            const float scale = sqrtf(1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;

            // TODO: speed this up
            X = (m(0,2) + m(2,0)) / scale;
            Y = (m(1,2) + m(2,1)) / scale;
            Z = 0.25f * scale;
            W = (m(1,0) - m(0,1)) / scale;
        }
    }

    return normalize();
}


// multiplication operator
inline Quaternion Quaternion::operator*(const Quaternion& other) const
{
    Quaternion tmp;

    tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
    tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
    tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
    tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);

    return tmp;
}


// multiplication operator
inline Quaternion Quaternion::operator*(float s) const
{
    return Quaternion(s*X, s*Y, s*Z, s*W);
}

// multiplication operator
inline Quaternion& Quaternion::operator*=(float s)
{
    X*=s;
    Y*=s;
    Z*=s;
    W*=s;
    return *this;
}

// multiplication operator
inline Quaternion& Quaternion::operator*=(const Quaternion& other)
{
    return (*this = other * (*this));
}

// add operator
inline Quaternion Quaternion::operator+(const Quaternion& b) const
{
    return Quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
}


// Creates a matrix from this Quaternion
inline Matrix4 Quaternion::getMatrix() const
{
    Matrix4 m;
    getMatrix_transposed(m);
    return m;
}


/*!
	Creates a matrix from this Quaternion
*/
inline void Quaternion::getMatrix(Matrix4& dest, const Vector3f& center) const
{
    float* m = dest.pointer();

    m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    m[1] = 2.0f*X*Y + 2.0f*Z*W;
    m[2] = 2.0f*X*Z - 2.0f*Y*W;
    m[3] = 0.0f;

    m[4] = 2.0f*X*Y - 2.0f*Z*W;
    m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    m[6] = 2.0f*Z*Y + 2.0f*X*W;
    m[7] = 0.0f;

    m[8] = 2.0f*X*Z + 2.0f*Y*W;
    m[9] = 2.0f*Z*Y - 2.0f*X*W;
    m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    m[11] = 0.0f;

    m[12] = center.X;
    m[13] = center.Y;
    m[14] = center.Z;
    m[15] = 1.f;
}



/*!
	Creates a matrix from this Quaternion
	Rotate about a center point
	shortcut for
	core::Quaternion q;
	q.rotationFromTo ( vin[i].Normal, forward );
	q.getMatrix ( lookat, center );

	core::Matrix4 m2;
	m2.setInverseTranslation ( center );
	lookat *= m2;
*/
inline void Quaternion::getMatrixCenter(Matrix4& dest,
                                        const Vector3f& center,
                                        const Vector3f& translation) const
{
    float* m = dest.pointer();

    m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    m[1] = 2.0f*X*Y + 2.0f*Z*W;
    m[2] = 2.0f*X*Z - 2.0f*Y*W;
    m[3] = 0.0f;

    m[4] = 2.0f*X*Y - 2.0f*Z*W;
    m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    m[6] = 2.0f*Z*Y + 2.0f*X*W;
    m[7] = 0.0f;

    m[8] = 2.0f*X*Z + 2.0f*Y*W;
    m[9] = 2.0f*Z*Y - 2.0f*X*W;
    m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    m[11] = 0.0f;

    dest.setRotationCenter(center, translation);
}

// Creates a matrix from this Quaternion
inline void Quaternion::getMatrix_transposed(Matrix4& dest) const
{
    dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    dest[4] = 2.0f*X*Y + 2.0f*Z*W;
    dest[8] = 2.0f*X*Z - 2.0f*Y*W;
    dest[12] = 0.0f;

    dest[1] = 2.0f*X*Y - 2.0f*Z*W;
    dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    dest[9] = 2.0f*Z*Y + 2.0f*X*W;
    dest[13] = 0.0f;

    dest[2] = 2.0f*X*Z + 2.0f*Y*W;
    dest[6] = 2.0f*Z*Y - 2.0f*X*W;
    dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    dest[14] = 0.0f;

    dest[3] = 0.f;
    dest[7] = 0.f;
    dest[11] = 0.f;
    dest[15] = 1.f;
}



// Inverts this Quaternion
inline Quaternion& Quaternion::makeInverse()
{
    X = -X;
    Y = -Y;
    Z = -Z;
    return *this;
}

// sets new Quaternion
inline Quaternion& Quaternion::set(float x, float y, float z, float w)
{
    X = x;
    Y = y;
    Z = z;
    W = w;
    return *this;
}


// sets new Quaternion based on euler angles
inline Quaternion& Quaternion::set(float x, float y, float z)
{
    double angle;

    angle = x * 0.5;
    const double sr = sin(angle);
    const double cr = cos(angle);

    angle = y * 0.5;
    const double sp = sin(angle);
    const double cp = cos(angle);

    angle = z * 0.5;
    const double sy = sin(angle);
    const double cy = cos(angle);

    const double cpcy = cp * cy;
    const double spcy = sp * cy;
    const double cpsy = cp * sy;
    const double spsy = sp * sy;

    X = (float)(sr * cpcy - cr * spsy);
    Y = (float)(cr * spcy + sr * cpsy);
    Z = (float)(cr * cpsy - sr * spcy);
    W = (float)(cr * cpcy + sr * spsy);

    return normalize();
}

// sets new Quaternion based on euler angles
inline Quaternion& Quaternion::set(const Vector3f& vec)
{
    return set(vec.X, vec.Y, vec.Z);
}

// sets new Quaternion based on other Quaternion
inline Quaternion& Quaternion::set(const Quaternion& quat)
{
    return (*this=quat);
}


//! returns if this Quaternion equals the other one, taking floating point rounding errors into account
inline bool Quaternion::equals(const Quaternion& other, const float tolerance) const
{
    return Myth::Math::equals(X, other.X, tolerance) &&
           Myth::Math::equals(Y, other.Y, tolerance) &&
           Myth::Math::equals(Z, other.Z, tolerance) &&
           Myth::Math::equals(W, other.W, tolerance);
}


// normalizes the Quaternion
inline Quaternion& Quaternion::normalize()
{
    const float n = X*X + Y*Y + Z*Z + W*W;

    if(n == 1)
        return *this;

    return (*this *= Myth::Math::reciprocal_squareroot(n));
}


// set this Quaternion to the result of the interpolation between two Quaternions
inline Quaternion& Quaternion::slerp(Quaternion q1, Quaternion q2, float time)
{
    float angle = q1.dotProduct(q2);

    if(angle < 0.0f)
    {
        q1 *= -1.0f;
        angle *= -1.0f;
    }

    float scale;
    float invscale;

    if((angle + 1.0f) > 0.05f)
    {
        if((1.0f - angle) >= 0.05f)  // spherical interpolation
        {
            const float theta = acosf(angle);
            const float invsintheta = Myth::Math::reciprocal(sinf(theta));
            scale = sinf(theta * (1.0f-time)) * invsintheta;
            invscale = sinf(theta * time) * invsintheta;
        }
        else // linear interploation
        {
            scale = 1.0f - time;
            invscale = time;
        }
    }
    else
    {
        q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
        scale = sinf(Myth::Math::PI * (0.5f - time));
        invscale = sinf(Myth::Math::PI * time);
    }

    return (*this = (q1*scale) + (q2*invscale));
}


// calculates the dot product
inline float Quaternion::dotProduct(const Quaternion& q2) const
{
    return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
}


//! axis must be unit length
//! angle in radians
inline Quaternion& Quaternion::fromAngleAxis(float angle, const Vector3f& axis)
{
    const float fHalfAngle = 0.5f*angle;
    const float fSin = sinf(fHalfAngle);
    W = cosf(fHalfAngle);
    X = fSin*axis.X;
    Y = fSin*axis.Y;
    Z = fSin*axis.Z;
    return *this;
}


inline void Quaternion::toAngleAxis(float& angle, Vector3f& axis) const
{
    const float scale = Myth::Math::squareroot(X*X + Y*Y + Z*Z);

    if(Myth::Math::is_zero(scale) || W > 1.0f || W < -1.0f)
    {
        angle = 0.0f;
        axis.X = 0.0f;
        axis.Y = 1.0f;
        axis.Z = 0.0f;
    }
    else
    {
        const float invscale = Myth::Math::reciprocal(scale);
        angle = 2.0f * acosf(W);
        axis.X = X * invscale;
        axis.Y = Y * invscale;
        axis.Z = Z * invscale;
    }
}

inline void Quaternion::toEuler(Vector3f& euler) const
{
    const double sqw = W*W;
    const double sqx = X*X;
    const double sqy = Y*Y;
    const double sqz = Z*Z;

    // heading = rotation about z-axis
    euler.Z = (float)(atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));

    // bank = rotation about x-axis
    euler.X = (float)(atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));

    // attitude = rotation about y-axis
    euler.Y = asinf(Myth::Math::clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f));
}


inline Vector3f Quaternion::operator*(const Vector3f& v) const
{
    // nVidia SDK implementation

    Vector3f uv, uuv;
    Vector3f qvec(X, Y, Z);
    uv = qvec.cross_product(v);
    uuv = qvec.cross_product(uv);
    uv *= (2.0f * W);
    uuv *= 2.0f;

    return v + uv + uuv;
}

// set Quaternion to identity
inline Quaternion& Quaternion::makeIdentity()
{
    W = 1.f;
    X = 0.f;
    Y = 0.f;
    Z = 0.f;
    return *this;
}

inline Quaternion& Quaternion::rotationFromTo(const Vector3f& from, const Vector3f& to)
{
    // Based on Stan Melax's article in Game Programming Gems
    // Copy, since cannot modify local
    Vector3f v0 = from;
    Vector3f v1 = to;
    v0.normalize();
    v1.normalize();

    const float d = v0.dot_product(v1);
    if(d >= 1.0f)  // If dot == 1, vectors are the same
    {
        return makeIdentity();
    }
    else if(d <= -1.0f)  // exactly opposite
    {
        Vector3f axis(1.0f, 0.f, 0.f);
        axis = axis.cross_product(Vector3f(X,Y,Z));
        if(axis.get_length()==0)
        {
            axis.set(0.f,1.f,0.f);
            axis.cross_product(Vector3f(X,Y,Z));
        }
        return this->fromAngleAxis(Myth::Math::PI, axis);
    }

    const float s = Myth::Math::squareroot((1+d)*2);   // optimize inv_sqrt
    const float invs = 1.f / s;
    const Vector3f c = v0.cross_product(v1)*invs;
    X = c.X;
    Y = c.Y;
    Z = c.Z;
    W = s * 0.5f;

    return *this;
}

/*
namespace Trigonometry
{

template<class T>
class Degrees
{

};

template<class T>
class Radians
{
public:
    Radian(T angle) :
        mAngle(angle)
    {
        //
    }

    Radian(const Degrees<T>& angle)
        mAngle(deg_to_rad(angle))
    {
        //
    }

private:
    T mAngle;
};

} // namespace Trigonometry
*/
} // namespace Math
} // namespace Myth

#endif // __MYTH_MATH_HPP_INCLUDED__

